REPORT  DOCUMENTATION  PAGE 


Form  Approved  OMB  NO.  0704-0188 


The  public  reporting  burden  for  this  collection  of  information  is  estimated  to  average  1  hour  per  response,  including  the  time  for  reviewing  instructions, 
searching  existing  data  sources,  gathering  and  maintaining  the  data  needed,  and  completing  and  reviewing  the  collection  of  information.  Send  comments 
regarding  this  burden  estimate  or  any  other  aspect  of  this  collection  of  information,  including  suggesstions  for  reducing  this  burden,  to  Washington 

Headquarters  Services,  Directorate  for  Information  Operations  and  Reports,  1215  Jefferson  Davis  Highway,  Suite  1204,  Arlington  VA,  22202-4302. 

Respondents  should  be  aware  that  notwithstanding  any  other  provision  of  law,  no  person  shall  be  subject  to  any  oenalty  for  failing  to  comply  with  a  collection  ot 
information  if  it  does  not  display  a  currently  valid  OMB  control  number. 

PLEASE  DO  NOT  RETURN  YOUR  FORM  TO  THE  ABOVE  ADDRESS. 


1.  REPORT  DATE  (DD-MM-YYYY) 

2.  REPORT  TYPE 

3.  DATES  COVERED  (From  -  To) 

20-09-2007 

Technical  Report 

15-Aug-2006  -  14-Aug-2007 

4.  TITLE  AND  SUBTITLE 

Workshop  on  Mobility  and  Control  in  Extremely  Challenging 
Environments 


5a.  CONTRACT  NUMBER 

W911NF-06-1-0374 


5b.  GRANT  NUMBER 


6.  AUTHORS 
lagnemma,  K  (editor) 


5c.  PROGRAM  ELEMENT  NUMBER 

611102 


5d.  PROJECT  NUMBER 


5e.  TASK  NUMBER 


5f.  WORK  UNIT  NUMBER 


7.  PERFORMING  ORGANIZATION  NAMES  AND  ADDRESSES 

Massachusetts  Institute  of  Technology 
Office  of  Sponsored  Programs 
Bldg.  El 9-750 

Cambridge,  M A  02139  -4307 


9.  SPONSORING/MONITORING  AGENCY  NAME(S)  AND 
ADDRESS(ES) 

U.S.  Army  Research  Office 
P.O.  Box  12211 

Research  Triangle  Park,  NC  27709-2211 


8.  PERFORMING  ORGANIZATION  REPORT 
NUMBER 


10.  SPONSOR/MONITOR'S  ACRONYM(S) 
ARO 


1 1 .  SPONSOR/MONITOR'S  REPORT 
NUMBER(S) 

51070-CI-CF.2 


12.  DISTRIBUTION  AVAILIBILITY  STATEMENT 
Approved  for  public  release;  Federal  purpose  rights 


13.  SUPPLEMENTARY  NOTES 

The  views,  opinions  and/or  findings  contained  in  this  report  are  those  of  the  author(s)  and  should  not  contrued  as  an  official  Department 
of  the  Army  position,  policy  or  decision,  unless  so  designated  by  other  documentation. 


14.  ABSTRACT 

The  attached  is  a  compilation  of  presentations  from  the  Workshop  on  Mobility  and  Control  in  Challenging  Environments, 
which  was  held  on  Oct  5-6,  2006,  at  Olin  College  in  Needham,  MA. 

Since  written  paper  submissions  were  not  required  for  the  workshop,  these  presentations  comprise  the  workshop 
proceedings. 


15.  SUBJECT  TERMS 

Mobility,  Control,  Unmanned  ground  vehicles,  autonomy 


16.  SECURITY  CLASSIFICATION  OF: 

a.  REPORT 

U 

b.  ABSTRACT 

U 

c.  THIS  PAGE 

U 

17.  LIMITATION  OF 
ABSTRACT 
uu 


15.  NUMBER  19a.  NAME  OF  RESPONSIBLE  PERSON 
OF  PAGES  Karl  lagnemma 

19b.  TELEPHONE  NUMBER 
617-452-3262 


Standard  Form  298  (Rev  8/98) 
Prescribed  by  ANSI  Std.  Z39. 1 8 


Report  Title 

Proceedings  of  the  Workshop  on  Mobility  and  Control  in  Challenging  Environments 

ABSTRACT 

The  attached  is  a  compilation  of  presentations  from  the  Workshop  on  Mobility  and  Control  in  Challenging  Environments,  which  was  held 
on  Oct  5-6,  2006,  at  Olin  College  in  Needham,  MA. 

Since  written  paper  submissions  were  not  required  for  the  workshop,  these  presentations  comprise  the  workshop  proceedings. 


PROCEEDINGS  OF  THE  ARO  WORKSHOP  ON  MOBILITY  AND  CONTROL 
IN  CHALLENGING  ENVIRONMENTS 


ARO  Award  Number:  W91  lNF-06- 1-374 


Workshop  Proceedings 


Prepared  for: 

U.S.  Army  Research  Office 
Systems  and  Control  Division 
P.O.  Box  12211 
4300  South  Miami  Blvd. 
Research  Triangle  Park,  NC  27709-2211 

Attention: 

Dr.  Randy  Zachery 

Tel:  (919)  549-4368,  Fax:  (919)  549-4354 
Email:  randy.zachery@us.army.mil 


Technical  POC: 

Dr.  Karl  lagnemma 

Department  of  Mechanical  Engineering 
Massachusetts  Institute  of  Technology 
77  Massachusetts  Avenue,  Room  3-435a 
Cambridge,  MA  02139 
Tel#:  617-452-3262  Fax#:  617-253-9637 


Email:  kdi@mit.edu 


TABLE  OF  CONTENTS 


Agenda 
Introduction 
Wilcox  Presentation 
Borenstein  Presentation 
Playter  Presentation 
Kelly  Presentation 
Tsiotras  Presentation 
Bevly  Presentation 
Urmson  Presentation 
Ray  Presentation 
Voyles  Presentation 
Lyons  Presentation 
Ayers  Presentation 
Rentschler  Presentation 
How  Presentation 
Frazzoli  Presentation 
Shiller  Presentation 
Spenko  Presentation 
Collins  Presentation 
Kobilarov  and  Sukhatme  Presentation 


Attendee  List 


ARO  Workshop  on  Mobility  and  Control  in  Challenging  Environments 


Olin  College,  Needham,  MA 
October  5  &  6,  2006 


Thursday,  October  5 

8:00  -  8:30 

8:30  -  8:50 

8:50  -  9:05 

9:05  -  9:20 

Registration 

Introduction  and  Workshop  Goals 

Workshop  Logistics  and  Welcome  to  Olin 
Workshop  Sponsor  Comments 

lagnemma 

Barrett 

Overholt 

9:20  -  9:45 

9:45-10:10 

10:10-10:35 

Design  1  Speaker  1 

Design  1  Speaker  2 

Design  1  Speaker  3 

Wilcox 

Borenstein 

Playter 

10:35-10:50 

Coffee  Break 

10:50-11:15 

11:15-11:40 

11:40-12:05 

12:05-12:30 

Controls  Speaker  1 

Controls  Speaker  2 

Controls  Speaker  3 

Controls  Speaker  4 

Kelly 

Tsiotras 

Beviy 

Urmson 

12:30-1:30 

Lunch  Presentation:  Extreme  Vehicle  Control 

McKinney 

1:30-1:55 

1:55-2:20 

2:45-3:10 

Design  II  Speaker  1 

Design  II  Speaker  3 

Design  II  Speaker  4 

Ray 

Voyles 

Lyons 

3:10-3:30 

Army  Perspective  on  Mobility  and  Control 

Witus 

3:30-3:45 

Coffee  Break 

3:45-4:45 

Moderated  discussion:  Challenges  in  mobility 
and  controls 

Pratt 

4:45-5:15 

Olin  College  Tour 

7:00 

Dinner 

Friday,  October  6 

8:30  -  8:55 

8:55  -  9:20 

9:20  -  9:45 
9:45-10:05 

Exotic  Environments  Speaker  1 

Exotic  Environments  Speaker  2 

Exotic  Environments  Speaker  3 

Exotic  Environments  Speaker  4 

Ayers 

Rentschler 

How 

Frazzoli 

10:05-10:20 

Coffee  Break 

10:20-10:45 

10:45-11:05 

11:05-11:20 

1 1 :20  -  1 1 :45 

Motion  Planning  1  Speaker  1 

Motion  Planning  1  Speaker  2 

Motion  Planning  1  Speaker  3 

Motion  Planning  1  Speaker  4 

Shiller 

Spenko 

Collins 

Kobilarov 

11:45-12:30 

Moderated  discussion:  Perspectives  on  mobility  Pratt 
and  control  in  challenging  environments 

12:30-12:40 

Wrap  Up 

lagnemma 

12:40-1:30 

Lunch  and  Adjourn 

Saturday,  October  7 


Visit  to  Team  O’Neil  in  New  Hampshire  for  hands-on  limit  handling 
demonstrations  and  discussions 

12:00  Meet  at  Littleton  Hampton  Inn,  caravan  to  Team  ONeil 

12:30  -1 :30  Discussion  of  driver  cues  and  vehicle  dynamics 

1 :30  -  3:30  Hot  laps  in  race  prepared  Ford  Escape,  SOF/ln-theater 

maneuver  demonstrations 

3:30  -  4:30  Post-demonstration  Q&A  and  facilities  tour 


CO  FrankUnWOlin 

Collegia  of  Engineering 


Workshop  on  Mobility  and  Control 
in  Challenging  Environments 


Olin  College,  Needham,  MA 
October  5  &  6,  2006 


Mobile  Robots — 
Historical  Perspective 


•  Early  mobile  robots 

-  SRI  Shakey,  1969 

-  Stanford  CART,  1970 

•  Classical  application 

-  Research  labs 

-  Hospitals 

-  Warehouses 

-  Factory  floors 

•  Operation  at  low  speeds  in 
structured,  benign 
environments 

-  Mobility  usually  not  a 
focus 


Mobile  Robots — 
Historical  Perspective 


•  Properties  of  (many)  early 
mobile  robots 

-  “Pizza  box  on  wheels” 

•  Little  consideration 
of  suspension  and 
drive  system 

-  Operation  in  static, 
planar  environment 

•  Simple  environment 
interaction  models 

•  Binary  obstacle/free 
space  representation 

-  Kinematic  control 


Mobile  Robots — 
“Next  Generation”  Applications 

•  Civilian  applications 

-  Hazardous/disaster  site 
inspection 

•  WTC,  Chernobyl, 

Katrina 

-  Planetary  exploration 

•  Sojourner,  MER,  MSL 

-  Passenger  vehicles 

-  Surgery  and  medicine 

-  Industrial  applications 

•  Underground  mine 
operations,  forestry, 
undersea  surveying 


Mobile  Robots — 
“Next  Generation”  Applications 


•  Military  application 

-  Scout/inspection  in 
dangerous  areas 

-  Inspection/disposal  of 
suspicious  objects 
(lED) 

-  Battlefield  rescue 

-  Surveillance  and 
reconnaissance 

-  Material  transport 


Mobile  Robots 


“Next  Generation”  Applications 


Operation  at  high 
speeds  in  unstructured, 
hazardous  environments 

-  Mobility  is  critical 

Requirements  of  next- 
generation  mobile  robots 

-  Design  for  high 
mobility 

•  Innovative 
suspension/drive 
system 

•  Design  for 
invertability, 
modularity 


Mobile  Robots 


“Next  Generation”  Applications 


Operation  in  dynamic, 
3D  environment 

-  Sophisticated 
understanding  of 
environment 
interaction 

•  Via  modeling, 
sensing,  or  design 

•  Non-binary 
obstacle/free 
space 

representation 

•  Geometric  and 
non-geometric 
hazards 


Mobile  Robots — 
“Next  Generation”  Applications 


Control  at  robot 

performance  limits 

-  Consideration  of 
robot  dynamics 

-  Consideration  of 
uncertainty 

-  Effect  of  robot- 
environment 
interaction 


Workshop  Purpose 


•  Workshop  purpose:  Survey  state-of-the-art  in  design, 
control,  and  motion  planning  of  mobile  robots  operating  in 
extremely  challenging  environments 

-  Outdoor  mobile  robots  on  Earth,  but  also... 

•  Planetary  surface  systems 

•  Underwater  robots 

•  Aerial  robots 

•  Surgical  systems 

•  Identify  fundamental  research  challenges  across  problem 
domains 

•  Identifying  innovative  potential  solution  paths 


ATHLETE:  An  All-Terrain 
Adaptive  Suspension  Vehicle 

Brian  Wilcox 

Autonomous  Systems  Architecture  and  Program 

Development  Office 

Jet  Propulsion  Laboratory 

5  Oct  2006 


ATHLETE:  the  All-Terrain,  Hex- 
Limbed  Extra-Terrestrial  Explorer 


Two  functional  prototype  vehicles  were  built  in  2005  as  part  of 
NASA  “Technology  Maturation  Program” 

Each  vehicle  is  ~850  kg,  hexagonal  frame  2.75  m  across,  ~300  kg 
max  payload,  top  speed  of  ~10  km/h  (2.8  m/s),  power  budget 
~5000W,  max  limb  tip  speed  at  full  extension  of  about  0.2  m/s 


Drive  off  the  dunes  -  not  sped  up  (~8  km/h) 


Where  we  are,  Where  we  want  to  be... 


Today  we  can: 

-  roll  10  cm,  stop  to  equalize  weight  on  each  wheel, 
repeat  N  times 

-  adjust  body  centering  and  pose  every  N  force 
redistribution  cycles 

Work  in  progress: 

-  continuous  weight  redistribution  and  body  reposing 

-  detect  anomalous  forces  on  a  wheel,  autonomously 
make  decision  to  put  some  or  all  of  its  weight  on 
other  wheels,  and  lift  and  advance  selected  wheel  in 
a  lightly-loaded  “terrain  following”  mode 

-  fully  autonomous  walking  on  extreme  terrain 

-  rappelling  on  steep  slopes 


ATHLETE:  Current  Capabilities 

•  Show  Movie 


Lessons  Learned,  and  Lessons  we 

expect  to  Learn 

Redistributing  weight  on  all  wheels  is  incredibly 
important 

-  imperceptible  pose  changes  every  10  cm  is  “all  the  difference” 
traversing  even  moderate  terrain 

Deciding  when  a  wheel  should  be  “relieved  of  its 
responsibility”  to  carry  its  share  of  weight  may  be  as 
simple  as  keeping  the  horizontal  force  on  each  wheel 
zero 

-  at  constant  speed  all  wheels  should  have  purely  vertical  net 
force 

-  if  negative  horizontal  force  component  appears  on  a  wheel, 
reduce  weight  on  that  wheel  until  horizontal  component 
disappears  (at  the  expense  of  higher  rolling  resistance  on  all 
other  wheels). 


Summary  and  Conclusions 


ATHLETE  provides  a  rich  environment  in  which 
to  study  the  adaptive-suspension  probiem. 

Simple  force-redistribution  and  body  reposing 
algorithms  are  very  effective. 

Six  (or  more)  smaller  wheels  and  motors  on 
limbs  can  have  less  mass  (and  cost)  than  three 
or  four  larger  wheels  and  motors  without  limbs, 
since  the  “walk  out”  contingency  option  means 
they  don’t  need  to  satisfy  all  the  worst-case 
requirements. 
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ARO  Workshop  on  Mobility 


The  OmniTread 
Serpentine  Robots 


Presenter: 


Johann  Borenstein 

Research  Professor 

University  of  Michigan 


Versatility 


U N  i  VERSITY  OF  M  JCI I  iG  AN 


We  have  developed  and  built  two  OmniTread  Models: 


Model  OT-8 

Can  pass  through  an 
8-inch  diameter  opening 


Model  OT-4 

Can  pass  through  a 
4-inch  diameter  opening 
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Capabilities:  OT-8 


♦ 

♦ 

♦ 

♦ 

♦ 


Can  travel  over  rocks  &  rubble 
Can  travel  over  deep  sand 
Can  travel  through  dense  underbrush 
Can  traverse  high  vertical  obstacles 
Can  traverse  wide  gaps 
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Capabilities:  OT-4 


♦  Can  travel  over  rocks,  gravel,  rubble 

♦  Can  traverse  high  vertical  obstacles 

♦  Can  traverse  wide  gaps 

♦  Can  travel  inside  pipes 

♦  Is  completely  untethered 

•  Batteries  last  for  up  to 
75  minutes  of  drive  time 
on  easy  terrain 


The  remainder  of  this  talk 
focuses  on  the  OT-4 


IJNiVERSiTY  OF  MiCl  liGAN 

Specifications  and  Nomenclature 


Parameter 

Specification 

structure: 

7  segments 

6  joints 

Drive  System: 

Tracks  on  all  sides.  Electric 
motor  in  center  segment 
drives  all  tracks 

Dimensions: 

Length  = 
Height  = 
Width  = 

Weight  = 

94.0  cm  (37”) 

8.5  cm  (3.35”) 

8.5  cm  (3.35”) 

4.0  kg  (8.8  ibs) 

Joints: 

Pneumatic  bellows  powered 
by  two  onboard  micro¬ 
compressors 

Joint  #1 


Forward 


Motor 
segment 


Seg.  #5 
Batteries 


Actuation 

segments 


Seg.  #6 
Air  compressor 

Seg.  #7 
Payioad 


Joint  #5 


Joint  #6 


Joint  #2 


Joint  #3 


Seg.  #1 
Payioad 
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Design  Features:  Maximum  Coverage  by  Tracks 


♦  Contact  between  environment  and  OT-4’s 
non-propelling  surface  impedes  motion. 

♦  Conversely,  contact  between  the 
environment  and  a  propulsion  surface 
produces  motion. 

♦  To  increase  propulsion  we  cover  all  sides 
of  the  OmniTreads  with  extra-wide  tracks. 

♦  Additional  advantages  of  tracks-all-around 


•  Massive  redundancy  in  case  of  track  failure 

•  OT-4  is  indifferent  to  rolling  over 

>  Roll-overs  are  inevitable  when  the  slender  bodies  of 
serpentine  robots  travel  over  rugged  terrain 

♦  Disadvantage:  High  power  consumption 

♦  Remedy  in  OT-4:  Track  clutches. 

•  28  micro-clutches  allow  operator  to  engage  and 
disengage  every  track  pair  individually. 
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Design  Features:  Pneumatic  Joint  Actuation 


♦  Pneumatic  joint  actuation  provides  natural  and  easily  controllable  compliance 


•  Natural  compliance  is  of  critical  importance,  since  propulsion  depends  on  optimal 
traction  between  propelling  surfaces  and  arbitrarily 
shaped  terrain  features. 

•  Maximal  traction  is  achieved  by  letting  Joints  go  limp, 
allowing  robot  to  conform  compliantly  to  the  terrain. 

♦  Joint  stiffness  can  be  controlled  in  real-time  to  any 
level  from  completely  compliant  to  completely  stiff. 


OmniTreads  achieve  maximal  traction 
and  propulsion  by  complying  naturally 
to  rough  terrain. 
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Control  of  the  Pneumatic  Joints 


Close-up  of  one  of  six  OT-4  joints 


Cross-section  of  the  OT-4  Joint 


aust 


Compressed  air 
supply  inlet 


An  array  of  8 
on-off  valves 
controls  one 
OT-4  Joint 


To 

jq  bellows  4 
Jo  bellows  3 
Jo  bellows  2 
bellows  1 
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Design  Features:  Drive  Shaft  Spine  and  Ciutches 


4  Single  motor  located  in  center 
segment  drives  articulated  drive 
shaft  spine  that  runs  through  all 
segments. 

•  Optimizes  weight  distribution 
(center  heavy,  ends  lights) 

•  Saves  weight,  volume,  and  power 

•  But  limits  range  of  motion  of  joints 

4  In  each  segment,  worm  on  drive 
shaft  spine  drives  four  worm 
gears,  which  transfer  power  to 
the  four  track  pairs  of  the 
segment  via  chains. 

•  Each  worm  gear  can  be 
disengaged  from  the  worm  by  a 
micro-clutch. 


Drive  motor 


Clutch 

motor 


■Track 

sprocket 


^Drive  shaft  spine 
to  front  segments 


Worm  gear 


Drive  shaft 
spine  to  rear 
segrnents 


Chain  to 
track  sprocket 


Track 

\ 


O 


Forward 

◄ - 


Motor 


Sprocket  2  /Track 
/  drive 
sprocket 

Chain 
Sprocket  1 
Inner 


-m3 


Worm 

gear 


Motor  segment  (4) 


U-joint 


Drive 

shaft 

spine 
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Micro-clutches 


♦  The  OT-4  has  28  micro-clutches,  one  for  each  side  of  each  segment 


•  Micro-clutches  disengage  the  bronze  worm  gear  by 
lifting  off  the  worm. 

♦  Main  advantages: 

•  Reduce  electric  power  consumption  by  disengaging 
tracks  that  are  not  in  use 

•  Reduce  overall  torque  on  the  drive  system  when 
disengaged 

•  Can  disengaged  damaged  branches  (there  are  28) 
of  the  drive  train 

♦  Main  disadvantage: 

•  Add  significant  complexity  to  hardware  and  software 

•  Add  some  weight 


Top:  left  side  of  clutch.  Bottom:  right  side 


Track  sprocket 
Chain 


Sprocket  2 


Micro-clutch 
motor  ^ 

/  > 
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Performance  Specifications 


steep  stair  climb  -  motion  sequence  based  on  “7G” 


Parameter 


Performance 


Control  &  Energy: 


Completely  tetherless 


Onboard  energy: 


Sufficient  for  75  min. 
continuous  driving  on 
smooth  terrain 


Can  climb  vertically  in 
pipes: 


4,  6,  and  8  inch  diameter 


Can  scale  vertical  walls: 


Up  to  40  cm  (16”)  high 


Can  bridge  gaps: 


Up  to  50  cm  (19”)  (more 
than  half  its  own  length) 


Speed: 


15  cm/sec 


OT-4  flexing  its  muscle. 

Here:  Lifting  up  three  distal  segments 
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Technical  Summary 


Rear  flipper- 
(fully  extended) 


Dual-head 
air  compressor 


1x2000  mAh  Li-Pol  & 
1x730  mAh  Li-Pol 
battery  pack 


Drive  motor 
40  Amp  electronic 
speed  controller 


1x2000  mAh  Li-Pol  & 
1x730  mAh  Li-Pol 
battery  pack 


Segment  7 

(can  carry  payload) 

Joint  6 

transmission  and  4  track 
clutches  in  all  seven  segments 


Dual-head 

4  pneumatic  ^ir  compressor 
bellows  per  joint 


Articulated  drive  shaft  spine, 
runs  through  all  segments 


Front  flipper 
(fully  extended) 


Cooling  fan 

Segment  4 

(drive  motor) 

Micro-processor  and 
controller  electronics 
in  all  seven  segments 


Joint  1 
Segment  1 

(can  carry  payload) 


Pneumatic  joint 
actuation  manifold 
with  8  valves  in 
Segments  1,2,3  and  5,6,7 


University  of  Mici  iigan 

The  OT-4  in  Action  -  Video  Ciips 
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Intelligent  Control  with  7G 

♦  Problem  with  serpentine  robots:  How  to  controi  many  degrees-of-freedom. 

•  Currently  three  operators  are  needed  to  control  the  13  DOF  of  the  OT-4. 


♦  Solution:  Al  Researchers  Biii  Hutchison  and  Betsy  Constantine  are 
developing  the  “7G”  self  learning  software  that  heips  the  OT-4  cope  with 
difficult  terrain. 


University  of  Mici  iiCAN 

Future  Work 


♦  Improve  and  harden  mechanical  system 

♦  Develop  semi-autonomous  control 

•  Currently:  3  operators  needed 

>  That  is  unacceptable 

•  One  operator  is  our  goal 

•  Computer-assisted  control 
requires  sophisticated  sensors 

>  And  sophisticated  self-learning  software,  e.g.,  7G 

♦  Integration,  commercialization 


U N  i  VERSITY  OF  M ICI I  iC  AN 

So  then,  when  it’s  all  done... 


♦  As  always,  the  visionaries  in  Hollywood  may  know  the  answer  long 
before  we  scientists  have  a  clue. 
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Robotics  at 
Boston  Dynamics 


Boston  Dynamics 


BigDog  -  Dynamic  quadruped 
LittleDog  -  Learning  Robot 
Legged  Robot  Mule 
RiSE  -  Climbing  robot 
RHex  -  Packable  rough-terrain 
MDMR  -  Snake 
NAV  -  Nano-Air  Vehiele 


QRIO  -  Sony  Dream  Robot 


RHex 


iS, 


Boston  Dynamics 


Rhex 


RHex  Devours  Rough  Terrain 


LittloDOQ  LGdrnitlQ  Robot  Boston  Dynamics  ^ 


•  Common  Platform  for  “Learning 

locomotion”  DARPA  program 
3  kg,  12  actuators 

•  Joint  angle  sensors 

•  Foot  contact  sensors 

•  Inertial  measurement  unit 

•  Wireless 


LittleDog  Learning  Robot 


Boston  Dynamics 


IPTO  PM  Larry  Jackal, 

Clip  from  Government  Testing 


Performers: 

•  CMU 

•  MIT 

•  IHMC 

•  Stanford 

•  U  Penn 

•  use 


Robotics  in  Scansoriai 
Environments 


Boston  Dynamics 


K.  Autumn 
M.  Cutkosky 
R.  Fearing 
R.  J.  Full 
D.  E.  Koditschek 
M.  Buehler 
A.  A.  RIzzi 


Lewis  &  Clark 
Stanford 

DC  Berkeley 
UC  Berkeley 
U  Pennsylvania 
Boston  Dynamics 
Boston  Dynamics 


RiSE 


Boston  Dynamics 


RiSE 


Boston  Dynamics 


BigDog  Goal:  Be  the  world’s  most  capable  dynamic  legged  robot, 
with  exceptional  rough-terrain  mobility,  autonomy  and  speed. 


Go  Where 
Soldiers  Qo 


j 

1 

1 

iBen 

BigDog 


Boston  Dynamics 


J^o.siion  Dyiiornics 


Risks  of  Bio-inspired  Design  Boston  Dynamics 


Common  Themes 


Boston  Dynamics 


•  Bio-inspired  design 

•  Meehanieal  design  ^  Intrinsie  mobility 

•  Active  modulation  of  contact  forces 

•  More  complex  terrains  require  more 
sensing  and  control 


Lateral  Wall  Reaction  Force 


Autumn  Lab 


Animal 

Pulls  Toward  Body 


Animal 


WaU 

Pulls  Away  from  Body 


Wall 


Wall 


Animal 


Animal  I" 

Pulls  Toward  Body 

WaU 

Pulls  Away  from  Body 


Versatile  Foot  Trajectory  Boston  Dynamics 

with  only  two  leg  motors: 

Leg=  rotating  (1dof),  compliant  four-bar  linkage  (Idol) 


Wall  Operation 


Ground  Operation 


CRANK  DIRECTION 


CRANK  DIRECTION 


Positive  RPM 


Pull  towards  body 


Negative  RPM 


Push  towards  ground 


Ground 


Legs 

Animal 


CONCORD 


FIELD  STATION 

,  HARVARD  UNIVERSITY 


Boston  Dynamics 
- BigDog 


c 

o 


Ct3 

:3 


o 

Ct3 

CO 

CO 

CD 


CD 

O 

c 

CD 


O 

O 

E? 
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Actuation 


Compliance 


Compliance 

+ 

Dissipation 


A.  Biewener,  D.  Lee,  Harvard  Concord  Field  Station 


Active  Force  Control 


Boston  Dynamics 


•  Control  traction  for  robust 
climbing 

•  Leg  Speed  adjusted  based 
on  sensor  readings 


Foot  Traction  and  Desired  Average  Traction 

T - 1 - 1 - i - 1 - ! - i - 1 - 


Desired  Traction 


I_ I_ I_ I_ I_ i_ I_ f  IL 

3.5  4  45  5  5.5  6  6.5  7  7,5  8 

Controller  State 


15 

10 

g 

03 

-  R 

o  ^ 

Ll- 

0 

Speed  Up 

Normal 

Slow  Down 

Flight 

0.3 

S  0.2 

las 

^  0,1 


DynBmiC  trot  co/7  fro/ boston  Dynamics 


Position 
control 


Torque 

control 


Stance  Leg  Control 

Torque  control  hip 

ab/adduction  keeps  body 
roll  at  level 

Position  control  flex/  extend 
joints  to 

-  sweep  foot  backwards 

-  adjust  the  compression  of 
leg  spring  for  body  pitch 
and  height  control 


Stance  Leg 


Boston  Dynamics 

BigDog  Active  Force  Control 


Shock  Loading  Improvement: 


Before; 


After  shock  pressure  reduction  and  load  control: 


DynBmiC  trot  co/7  fro/ boston  Dynamics 


Swing  Leg  Control  -  Part  1: 

Swing  legs  move  with 
respect  to  an  “ideal” 
robot  that  is  straight  and 
level 


DynBmiC  trot  co/7  fro/ boston  Dynamics 


Swing  Leg  Control  -  Part  2: 


Dynamic  sideways  balance 


Moyement  of  stance  legs 
affect  placement  of 
swing  legs 


Boston  Dynamics 

Increasing  Terrain  Challenges 


•  Steady  state  behavior 

•  Reactive  control 


•  Low  terrain  sensing 


Boston  Dynamics 

Increasing  Terrain  Challenges 


Steady  state  behavior 
Reactive  control 

Low  terrain  sensing 


•  Steady  state  behavior 
with  transitions 

•  Recovery 

•  Odometry 

•  Medium  terrain 


sensing 


step  Obstacle 


Boston  Dynamics 


Jumping 


Running  at  3.3  m/s  (7.4  mph) 

Does  not  include  engine  weight  (301bs) 


Boston  Dynamics 


Jumping 


Boston  Dynamics 


Boston  Dynamics  W 

Increasing  Terrain  Challenges 


Indiscriminate  foot  placement 

Intermittent  foot  placement 

Precise  foot  placement 

•  Steady  state  behavior 

•  Steady  state  behavior 

•  Unsteady  dynamics 

•Reactive  control 

with  transitions 

•  Predictive  control 

•  Low  terrain  sensing 

•  Recovery 

•  Odometry 

•  Accurate  odometry 

•  High  Terrain  sensing 

•  Medium  terrain 


sensing 


Boston  Dynamics 


BfeliiBereilfSn  and  Exception 


in  Challenging 
Environments 


.  Alonzo  Kelly 
Carnegie  Mellon 

National  Robotics  Engineering  Center 


Challenges  of  Challenging  Terrain 
Stability  Margin  Estimation 

Trajectory  Generation 

■  Instrument  Placement 
■^ath  Following 

■  Cluttered  Terrain  Planning 

■  Obstacle  Avoidance 


Exception 

Difficulty  level  is  high  =>  autonomy  will  fail 
more  often 

Risk  level  is  high  =>  results  can  be 
disastrous. 


Fault  tolerance,  not  algorithmic 
sophistication,  will  enhance  robot 
survivability. 


•  (i.e.  prediction  and  selection) 

•  Models  must  be  3D,  perhaps  volumetric 

•  Wheel-terrain  interactions  are  central  to 
motion  prediction. 

•  Terramechanical  properties  are  difficult  to 
measure  with  noncontact  perception 
sensing. 

It  takes  more  computation  to  produce  a 
lower  quality  result. 


Themes 


*  1 :  Need  fast,  robust  systems  to  detect  and 
react  to  autonomy  failure. 


•  2:  Adequate  predictive  models  are  both 
necessary  and  enabling. 


Lack  of  a  model  is  a  predictable 
“disturbance”. 


Estimatioh 


A.  Diaz-Calderon,  A.  Kelly  “Online 
Stability  Margin  and  Attitude  Estimation 
for  Dynamic  Articulating  Mobile 
Robots”  IJRR,  Oct/Nov  2005. 


Robot  rollovers  happen. 
Risk  is  increased 

■  on  slopes  ^ 

and/or 

■  at  high  speeds 

Field  robots  must  become 
competent  despite  these 
dangers. 


These  three  robots  rolled 
within  3  weeks  of  each  other 
in  2003. 


Approach 


Basic  idea  developed  for 
legged  robots  long  ago. 

Compute  inertial  properties 
(eg)  in  real  time. 

Predict  wheel’  liftoff  rather 
than  rollover. 

■  Don’t  need  to  know  inertia. 

Measure  specific  force 

■  Immune  to  drift. 

■  Don’t  need  to  know  attitude. 

■  Can  actually  measure  it 
anyway. 


lementation 


IMUs,  gyros,  odometry,  articulation  sensing  etc. 
Kalman  Filter 

^^gyro  or  speed/steer  ^  ^  ^ 

tp  =  t„+  ac +2(b„x  Vc+  aprX  rc +a)„xco„x  rc 
^  Q  —  CT  ^  ^  Q  ^  ^ 

* 

inclinometer  numerical  d/dt  eg  kinematics 

r  =  position  a  =  acceleration  co  =  ang.  velocity  a  =  sensor 

V  =  velocity _ t  =  spec,  force _ a  =  ang.  accel. _ c  =  eg 

•  Predict  specific  force  that  would  be  observed  at  the  eg. 

•  Compare  to  the  support  polygon 

■  Can  all  be  computed  in  the  body  frame. 


Developed  for 
industrial  lift  trucks 

Never  put  it  on  a 
UGV  (outdoor 
robot). 


0.1000  Frame=2 


Simulated  ResuBt 

Vehicle  without 
Stability  Governor 

•  Load  weight:  SOOOIbs 

•  Speed:  constant  5mph 

•  Curvature:  constant 

•  Lift  height:  340  inch 

apianT_oui  iime=  u.uuuu  t-rame^i 

Vehicle  with  \ 

Stability  \ 

Governor  \ 


^jectory  Generation 


T.  Howard,  A.  Kelly  “Optimal  Rough 


Terrain  Trajectory  Generation  for 
Wheeled  Mobile  Robots  Mobile 


Robots”,  to  appear  IJRR  2006 


•  High  fidelity  simulation  is  routinely  used  in 
off  road  obstacle  avoidance. 

•  Use  these  existing  models  in  lower  level 
motion  control. 

^^Becomes  the  core  capacity  to  move  the 
^  vehicle. 

■  Used  by  all  higher  level  “planners”. 


NymerTcal  Model  Inversion 


•  Terrain  is  not  known  in  - 
analytic  form. 

•  Terrain  following  is  an 

important  and  g  '* 

preditable 

“disturbance”.  “ 


Goal  State 
(x,y,e,K,V)f 

^ - - 

D 

Start  State 
g  (x,y,e,K,V)o 


Optimal  Control 


Optimize 


J  =  (|)[x(t^)]  +  |L(x,  u,  t)dt 


■  Subject  to: 

tf  free 

X  =  f(x,u,t) 

x(to)  =  Xg  x(tf)  =  X 

u(t)  <u  (t)  |u(t)|<u  (t) 

_max  L  I  _niax 

^  A  natural  formulation  with 
standard  numerical 
approaches  for  sampled 
solutions. 

^  Search  a  function  space. 


Performance 

Index 


Nonlinear  Programming 


Optimize 


|^J(p)  _=  (|>(pjf)  +  jL(p,  t)dt 


Free  duration 

Dynamics 

Boundary 

Conditions 

Input  Limits 


Subject  to: 
tf  free  " 


f(P,  to,tf)  =  0 


P3 

U(E) 

P2 

P1 

|P|  ^  Pmax 

•  Easier  (less  dof)  and 
produces  continuous 
solutions. 

•  Search  a  parameter 

space.  w 


COTPctive  Trajectories 


Constrained  Optimization  Path  Tracking  Experiments 
Jl  ne  28th,  2U06 


( ariH‘v^k‘I\Mlon 
THE  ROBOTICS  INSTITUTE 


DPI  Path  Tracking  Experiments  Juiy  28  2006.avi 


Search  Space  Design 


States: 

■  discretize  regularly  (lattice). 

Controls: 

■  compute  connectivity  via  exact 
solutions  in  finite  neighborhood. 


Prune  controls  based  on: 

■  Redundancy 

■  Feasibility 


4  connected,  1  deep 


16  connected,  2  deep 


8  connected,  1  deep 


24  connected,  2  deep 


ReSrControl  Set 


Encapsulates  the 
essential  connectivity  of 
state  space  subject  to 
nonholonomic/dynamic 
etc  constraints. 

This  can  be  generated 
automatically  given  a 
trajectory  generator. 

This  is  (implicitly)  copied 
everywhere  to  generate 
the  search  space. 


NOTE:  All  headings  shown  in  one  layer 


Con^sions 

Stability  Margin  Estimation 

■  Its  just  code! 

■  Useful  if  (when)  autonomy  fail's. 

Trajectory  Generation 

■  Core  capacity  to  plan  end  execute  any 
feasible  motion. 

>  Many  planners  can  be  built  over  top. 


Aggressive  Maneuvering  of  Ground  Vehicles  over 
Rough  Terrain  and  Uncertain  Environments 
Key  I  ssues  and  Possible  Approaches 

Panagiotis  Tsiotras 
School  of  Aerospace  Engineering 
Georgia  I  nstitute  of  Technology 
Atlanta,  GA  30332-0150 


ARO/MIT  Workshop  on  Mobility  and  Control  in  Challenging  Environments 

Olin  College,  October  5-6,  2006 
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Why  Autonomous  Vehicles? 


^  Car  accidents  result  in  more  than 
40,000  deaths  and  2,780,000  injuries 
each  year  in  the  US  alone 

^  Car  accidents  are  the  leading  cause 
of  mortal  injuries  globally 

^  Leading  cause  of  death  between 
ages  3-33 


Fatat  Crashes,  1^75-2004 

gr  F$(hI  CfuhM 


Distribution  of  global  injury  mortality  by  cause 


Other 


Suicide 


unintentional  injuries 

18.1% 


16.9% 

s. 


Violence 

10.8% 


Fires 

6.2% 


Falls 

7.5% 


Poisoning 

6.7% 


3,4% 


Other 

intentional  injuries 
0.2% 


Road  traffic  injuries 


Drowning 

7.3% 


22.8% 


By  2020,  road  traffic  accidents  will  be 

the  3rd  leading  cause  of  death 

due  to  injury  and  disease 
combined  (WHO) 
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Military  Applications 


♦  DARPA  2005  GC:  131.2  miles  of  unpaved  course 
under  10  hours  (Mojave  dessert) 

♦  Winner  (Stanley)  average  speed  19mph 


^  Navigation  in  uncertain  and  dangerous 

environments 

^  Minimize  exposure  in  danger  zone 

^  Maximize  speed 


Effect  of  Speed 
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^  Difficulty  to  hit  a  target  increases 
additively  with  speed. 

^  As  many  deaths  from  vehicle- related 
accidents  as  from  hostile  action. 


MVA  Deaths 

Hostile  Deaths 

245 

3 

345 

18 

337 

344 

377 

737 

356 

739 

1660 

1841 

2001-2005  Data 


^  Driver-assist  aigorithms  and/or 
realistic  training  can  prevent  this 
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Fast  Driving  over  Rough  Terrain? 


^  (Expert)  humans  do  it  all  the  time 
^  Rally  racing 

^  Rough,  loose  terrain,  ice  @  lOOmph 

^  Large  slip  angles  (forget  “nonholonomic 
constraints") 

^  Different  than  closed- track  ( FI,  NASCAR) 
^  What  can  we  learn  from  these  experts? 


1 

E  ::o;  i  T]^©[]l](n)©D©g|^ 

=  S^:h[M]]  of  Attrcj.spuce  Engineering 


Rollover  Avoidance? 
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Friction  is  Key  (what  else?) 


Friction  force 
Normal  force 


HeMonshlpof  mu  and  and 


Relationship  of  mu  and  velodty 


Notoriously  difficult  to  characterize!! 


1 
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The  Friction  Circie 


-500 


0  500  1000  1500  2000  2500 

f  (N\ 


2000 


1500 


1000 


500 


-500 


-1000 


-1500 


-2000 


-2500 


-2000 


-1500 


-1000 


Average  Driver 


Expert  Driver 
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A  Dynamic  Tire  Friction  Model 


Wheel  with  IlI^^)ed  fiicdon  F  under  braking 


dt  at  '  ac 

^*(t)  =  ^  w(t,C)/ii(C)dC,  i  =  x,y 

Mz{t)  =  -j^‘^/*»«,0/»(o(|-<)tic 
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Captures  all  conditions 
of  friction  circle 


Fy_(N) 


Steady- State 


I  .  I  .  I  .  I  . 

IS  Zl  30  40 

slqi  an^e  (deg) 


Lumped  Model 


m 

F(t) 

CoC^r) 


Vr  —  Co{Vr)z{t)—K,(t)\ijJr\z{t) 
-Fn((Toz{t)  +  aiz{t)  +  a2Vr) 
<^0\Vr\ 

gM 


So  zit,0fk{C)dC 
So^{t,OMC)dC 
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Experimental  Validation 
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How  About  Control? 


I 


^  Assume  point-mass  model  for  velocity  control 


B 


Zl  =  <22, 


-  (^)  ■ 


Z2  =  U\1 


u  e  [“1,  +1] 


^  Assume  bicycle  model  for  posture  control  (plus  load  transfer) 


Northing  (m) 
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Application  to  FI 


I 

DCSl 
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Posture  Control 


Rear  lateral  slip  determined  by  vehicle  state 
Front  wheel  may  generate  any  force  in  the  fc 

Control  I  nputs 

-  front  steering  angle 

-  front  &  rear  wheel  torque  (slip) 
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Simulations 


Aggressive  Maneuvering 
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^  What  cost  does  the  race  driver  tries  to  minimize/ maximize? 
^  Minimunn-Time,  Maxi munn- Speed,  or  ...? 


Minimum  Time 


Maximum  Exit  Velocity 
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Skidding  can  be  Optimal 


Maximum  Exit  Velocity 


A  Menagerie  of  Maneuvers 
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Technique 

When  is  it  used? 

Driver’s  actions 

Left  fbot 
Braking 
(LFB) 

When  weight  transfer 
from  front  to  rear  axles 
and  vice  versa  is  required 

Simultaneous  application  of  throt¬ 
tle  and  brakes  to  fine  tune  the  dis¬ 
tribution  of  torque  between  front 
and  rear  axles 

Slide  TWning 
(Normal 

Drift) 

Entering  fast  on  a  wide 
turn;  need  to  yaw  fast 

Short  application  of  brakes  while 
cornering  to  initiate  sliding; 
straighten  wheels  and  stop  braking 
once  sliding;  accelerate  when 
aligned  to  the  exit 

lYail  Braking 

When  carrying  too  much 

speed  entering  a  turn; 
need  to  yaw  fast 

Brake  hard  before  comer;  start 

steering  while  slowly  releasing 
brake 

Pendulum 

(Scandinavian 

Flick) 

Extremely  tight  comers; 
normal  drift  not  enough 

Initial  slide  to  the  opposite  direc¬ 
tion  of  the  corner  to  reduce  speed 
as  necessary;  turn  into  the  corner 
accelerating  fast  and  LFB  to  con¬ 
trol  understeer. 

Handbrake 

Cornering 

Tight  turns  and  not 
enough  space  for  the 
pendulum  mode 

Apply  handbrake  to  reduce  rear 
wheel  side  traction;  initiate  drift  by 
turning  the  steering  wheel. 

Power 

Oversteer 

Slide  for  a  RWD  vehicle 

Accelerate  hard  to  reduce  rear 
wheel  side  traction;  initiate  drift 
turning  the  steering  wheel. 

Load  transfer  extremely  important 

NOTE: 

Fine  tuning  of  accelerating/ brake  torque 
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Two  Examples 


Trail- Braking  Pendulum 
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Trail  Braking 


TiSl?' 
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Pendulum 
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A  Natural  Approach 


^  Construct  a  library  or  nnaneuvers 

^  Schedule  them  via  a  maneuver  automaton  (ala'  Frazzoli  et  al) 


m*  =  mi  o  m3  o  mi  o  m2  o  7714  o  •  •  • 


Advantages 

^  Easy  to  implement 

^  Mimics  expert  race  driver 

Challenges 

^  Need  a  good  parameterization 

^  Robustness 

^  Triggering  (environmental 
awareness) 


Case  Study:  TB 
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fixed  fi«9flfli5^i(9fi'siti(?r 

- V^fre^ffr-ee— ► 

Zero  slip 


State  constraint 


control  constraint: 

<tti<+l,  i  =  S,T 


Cost  function: 
final  time 


fixed  initial  position 
Vo  =  70km/h 
Zero  slip 


a 
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Case  Study:  TB 


DCSL. 
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Comparison 


3f) 
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Trail  Braking 
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Multi- res  Path  Planning 


Motivation:  -  Limited  on-board  comp  resources 


Multi  res/ multisensor 
data  processing 
and  integration 


0  CCRS / CCT 


SchcH]]  of  Aiinci.spiire  EngLneHMing  Wavelet- Based  Cell  Decomposition 


J^OO 


+  00  +00 
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The  wavelet  decomposition 


2*^min  — 1 


3  J-1  2J-1 


/(a;.y)=  1]  4w®  S') 


fc^=0 


induces  a  cell  decomposition  of  W 


i — 1  J — «/min  — 0 


Cd  =  e  AC'^“'”  ®  •  •  •  e  AC^ 


^max  1 


E  ::o;  i  T]^©[]l](n)©DcS^^ 

=  S(:h[wil  of  Englntwring 


Example 


Real  topographic  data 
Objective;  Avoid  blue  areas 
Original  data  512x512 
Planning  over  cells  of  64  and  8  units 


TiSl?' 


E  ::o;  i  T]^©[]l](n)©D©g|^ 

=  S^:h[M]]  of  Attrcj.spuce  Englnwering 


Animation 


::o;iT]^©[]l](n)©D©g|^ 

S(:h[W]l  of  Englntwring 


^  Expand  operational  regime  of  passenger  vehicles 
^  Driver-assist  “drive  by  wire" 

^  Driver  management  systems 
^  Posture  control 


cortrol: 
hand  brake  lo 
mduca  impact 


% 

control: 

$Qft  to  allow 
steering 
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intelligent 

steering- 
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David  M.  Beviy 
Assistant  Professor 


GPS  for  Navigation,  Modeling,  and 
Control  of  UGVs 


Department  of  Mechanical  Engineering 
Auburn  University,  AL  36849-5341 

Director  of  Auburn  University's 

GPS  and  Vehicle  Dynamics  Lab  (GAVLAB) 


GPS/INS: 

The  Perfect  Complement 


GPS  (Low  Frequency  Sensor) 

INS  (High  Frequency  Sensor) 

■  Limited  to  1-20  Hz 

■  Stable  over  long  periods  of  time 

■  Stochastic  zero  mean  noise 

■  Unbiased 

■  Noisy 

■  Higher  output  rates  available 

■  Drift  over  long  periods 

■  Noise  due  to  vehicle  dynamics 

■  Biased 

#  The  combination  provides  a  high  update  rate, 
low  noise,  unbiased  measurement  solution 

^  Various  Integration  Techniques 

■  Tightly  Coupled  (offers  INS  aiding  with  limited 
satellites,  i.e.  Urban  Canyons) 

■  Ultra-Tightly  Coupled  (offers  improved  noise  immunity 
and  instantaneous  reacquisition  after  short  outages, 
i.e.  jamming  or  foliage  environments) 
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IMU  Modeling 


#  Develop  models  to  predict 
performance 


#  Model  various  grade 
(including  MENS  based) 
inertial  sensor 
characteristics 


^Validate  simulated  data 
with  experiments 

^meas  ^  '  ^act  +  Konst  +  Kalk  +  ^ 
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■iancep(x) 


Low  Grade  IMU 


T  (sec) 


Mid-Grade  IMU 


Gyroscope  Comparison 


’Blending  GPS  and 
consumer  grade  gyro 
bounds  heading  error 

’Pure  tactical  grade  gyro 
integration  has  less  error 


0  0.1  0.2  0.3  0.4 


Classification  Characteristics  Used  to  Categorize  Rate  Gyros 


Rate  Gyro 

Consumer 

Attribute 

Units 

Specification 

Random  walk 

°/sec/VHz 

0.05 

Bias  Time  Constant 

sec 

300 

Bias  Variation 

7hr 

360 

Tactical 

Random  walk 

7sec/VHz 

0.0017 

Bias  Time  Constant 

sec 

100 

Bias  Variation 

7hr 

0.35 

-  Rugga  Kutta  Order  Four  (Consumer  Grade) 

-  2  State  Kalman  Filter  (Consumer  Grade  coupled  with  GPS) 

-  3  State  Kalman  Filter  (Consumer  Grade  Coupled  with  GPS) 

-  Pugg  Kutta  Order  Four  [Tactical  Grade) 


15  20  25 

Time  (sec) 


Lateral  Errors  When  Dead 
Reckoning  (No  GPS) 


1.5 


0.0 


cd 

w  -0.5 
^  -1.0 


-1.5 


3  Tests 

•  Vx=2  m/s 

•  Line  Tracking 

Results 

•  Errors  <  0.3m 

for  40  sec 


Error  Analysis: 
yE  ~^xWe  ~  ^\j/  'sj'^s  ^ 


3 
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INS  Aiding  Using  External  Aids 


#  Laser  scanners  provide  environment 
information  which  can  be  used  to 
navigate  in  a  defined  corridor 

Provides  ability  to  estimate  the  following 

■  Velocity,  local  heading 

■  Local  lateral  error  (for  use  in  some 
controllers) 

■  Lateral  vehicle  movement  (also  makes 
vehicle  sideslip  and/or  lateral  velocity 
observable) 


#  Currently  using  well  defined  aids  to 
define  corridor 
■  Walls,  lane  markings,  etc. 

■$>  Preliminary  experiments  run  in  hallway 
and  on  test  track 
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Unstructured  Object  Registration 


#How  to  do  object  registration/aiding  in 
unstructured  and  dynamic  environments 

■  Use  trees,  road  edge,  etc  to  define  corridor 


and  aid  INS 


Images  from  UGV  Test  Course  at  Auburn 


ir  A  ^ 


steering  Control  Strategies 


■$>  Waypoint 

■  Easier  to  implement  and 
tune  (by  hand) 

■  Requires  fewer  model 
parameters 

Driving  to  waypoints  is  not 
best  control  method 

#  Results  in  decreased 
tracking  accuracy 

■  Vehicle  oscillates  more 

X  X 


X 


#  Line  Tracking 

■  Need  good  model 

■  Requires  more  parameters/states 

■  Harder  to  define  error 

■  Provides  improved  tracking 

#  Two  errors  are  important 

■  Heading 

■  Lateral  position 
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ATV  Way-Point  Controller 
Performance 


Various  Yaw  Dynamic  Modeis 


nii 


Bicycie  Modei 


m 

S(s) 


a  C^fS  + 


aC  f  —c,C  r 

a  /  \  a  / 

mV 


2  Cal  7  +  mc^ 


( 


2  ^2  ^ 


45"  -f  ^ 


mV 


■s  + 


\ 


CQC2-c^mV  -c{ 
mV‘ 


J 


DC  Gain:  R,,  = 


F 


”  L  +  K^sV" 


Neutrai  Steer  Modei  (k^,s=0) 


m 

Sis) 


aC 


af 


T  ^2 

IzS+  2 


V 


Kinematic  Modei 


R  =  —S 
L 


Identification  of  ATV  Dynamics 

^ - 


r  K{s  +  n,) 

5  +  2C,co^s  +  0)1 


Frequency  (Hz) 


Steer  SysID  Test 


^  2 


u> 

0 


-2 


-4 


-  Steer  Angle 

..III  I  -  Yaw  Rate 

UliilM""" 


1650  1660  1670  1680  1690  1  700  1710  1720 


4 


-4  I - . - : - 

1673  1674  1675  1676 

Time  (s) 


4 

2 


-2 


-4 1 - ^ - 

1690  1691  1692 


Typical  Yaw  Vehicle  Model 


V  =  velocity 

r  =  yaw  rate 

i//  =  vehicle  heading 

S  =  steer  angle 

p  =  body  side  slip 

a  =  tire  side  slip 

C„  =  cornering  stiffness 


Lateral  Force, 


^7 


Tire  Behavior 


F.=C,-s 

Fy=C„-a 


>  Linear  for  small  slip 
angles 

>  Saturates  at  higher  slip 
angles  (vehicle  slides) 

>  Varies  with  loading 

>  Corning  stiffness 
depends  on  tire  only 

>  Peak  force  changes  with 
surface  (|a)  and  tire 


Pacejka  "Magic"  Tire  Model  (sae  Paper  870421  ) 


Roll  Modeling  (for  anti-roll) 


Developed  simple 
vehicle  models  to 
study  effect  of 
various  properties 
on  handling  and 
roll  dynamics 


Roll  Rate  = 


W,-h\ 


-  Roll  Rate  -a  k^=  roll  stiffness 


k^+k^-Wrh\ 


a  )  ay  =  lateral  accerlation 
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[i[deg)  Yaw  Rate  (deg/secj 


Vehicle  Model  Validation 


Crown  of  track  visible 
In  roll  measurement 


Model  Matches  Actual 


Simulated 

Experimental 


Yaw  Rate 
(deg/s) 


Vehicle  Rollover  Simulations 


Constant  Radius  -  Steady 
acceleration  maneuver 

Studies  the  effect  of  weight  split  on 
vehicle  stability 


Constant  SSF 


Varying  SSF 
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Small  Low-Cost  Vehicle  GPS/INS 
System 


#ESC  Scaled 
Experiment  Testbed 


#  CG  Relocator 

GAVLAB  GPS/INS  System 
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Rollover  Mitigation  (Using 
GAVLAB  GPS/INS  System) 


Without  ESC  II 


With  ESC  I 


Total  System  Dynamics  (for  Yaw 
Control) 


#>Steering  s _  k,  ^ 

■  order  u  TgS+\ 

♦  Neglecting  motor  dynamics 

^Vehicle  r_  K{s+n^) 

■  Order  s  s^+ 2(;^co^s + wl 

#Error 

■  Heading  (pt  order) 

■  Lateral  Position  (2"<^  Order)  y  «  v^y/ 
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steering  Control  Strategies 


^Classical 

■  Cascaded 


♦  Design  (need  good  model) 

♦  Hand-Tuning 

estate  Feedback 


■  Need  Good  Model  for  Design 

■  Need  Estimator  (also  requires  good  model) 


Off  Road  Line  Tracking  Control 


- 

n  o  ^ 

V.y 

Mean  =  5  mm  lo=3  cm  1  Foot 

^  0-6 

^  0.3  - 

(D 

0.0  - 

-0.3  - 

0  50  100  150  200 
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Control  Input  Lateral  Error  (m) 


Effect  of  Velocity  on  LQR  Closed 
Loop  Bandwidth  u = 


Closed  loop  bandwidth  increases  with  velocity  for  a 
given  set  of  LQR  control  weights 

Closed  loop  bandwidth  approaches  2"*^  order  tractor 
bandwidth  dynamics  at  higher  speeds 


Control  Input  Lateral  Error  (m) 


High  Speed  Control 


# Accurate  control  at  full  range  of  tractor 
speeds  (using  correct  vehicle  dynamics) 


Vx=5  m/s 


Vx=8  m/s 


l-H 

w 

h-1 


Additional  Loads 


Can  a  model  capture  the 
behaviors  actually  seen 
from  real  data? 

Models  useful  to  steering 
control  describe  the 
turning  rate  of  the  tractor 
from  a  given  steering 
angle 


Implement  described 
using  a  tire  model 


Tire  Implement 


2 


Axij 


Effect  of  Hitched  Implement 
Dynamics 


Root  Locus 


Root  Locus 


With  Correct 
Implement  Model 


Neglecting  Impiement 
Dynamics 
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EKF  Hitch  Estimation 


Estimation  of  Hitch 
Cornering  Stiffness 
from  Implement 


Control  with  Correct  and 
Incorrect  Implement  Model 


Ir  ^  ^ 


Instrumented  Test-Vehicle  Used 
for  Parameter  Estimation 


CrossBow  IMU  # 

■  3  Axis  Acceleration 

■  3  Axis  Rotation  Rate 
DATRON  Velocity 

■  Longitudinal  Speed  # 

■  Lateral  Speed 


#  CAN 


Starfire/Beeline  GPS 

■  Position  &  Velocity 

■  Course 

■  Heading  &  Roll 

On  Board  PC 

■  Data  Logging 

■  Real  Time  Analysis 


■  Wheel  Speeds 

■  Steer  Angle 


Dash 


Experiment  Site  and  Tests 


Yaw  Rate  (deg/sec)  sideslip  (deg) 


Vehicle  State  and 
Parameter  Estimation 


#  Estimate  vehicle  parameters  and  states  that  may  otherwise: 

■  be  difficult  to  measure 

■  require  expensive  sensors 


#  Uses  GPS  and  low  cost  inertial  sensors  with  a  vehicle  test-bed 


Estimation  of  Understeer  gradient 


Force  and  Mass  Estimation 


#  Use  vehicle  measurements 
to  estimate: 


■  Drive  Force 

■  Vehicle  Mass 

■  Air  Drag 

■  Rolling  Resistance 


I'Jrivc  OtiJipisriiHiin 
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si  (s/Bep)  j 


Tire  Estimation 


Simulated  vs.  Experimental 
Vehicle  Data 


Time  (s) 
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Rear  Lateral  Force  Front  Lateral  Force 


Experimental  Data  vs. 
Estimated  Tire  Model 


•  Measured 

•  DugoffFit 

- T - - - T - - - - 

h 

-10  -5 

0  5  10  15 

Front  Slip  Angle  (deg) 

10 


Rear  Slip  Angle  (deg) 


On-line  Tire  Curve  Estimation 

lOr - - 1 - 1 - i - 1  lOp - ^ ^ ^ ^ - r 


Front  Tire  Slip  Angle  (deg)  Rear  Tire  Slip  Angle  (deg) 
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§ 


o 

Pm 


H  0 


cd 

(D 


-5 


i 


Xx>^  X  X  ^XxX 


X  Filtered  Experiment 

•  Experimental 

•  DugoffFit 


-40  -30  -20  -10 

Rear  Tire  Slip  Angle  (deg) 


Our  newest  high-speed  UGV  (Trained  K-9): 


Auburn  University 


GPS  AND  Vehicle 

Dynamics  Lab 


http://gavlab.auburn.edu 


High-Speed  Driving  of 
Prescribed-Routes 

Chris  Urmson 


Chris  Urmson 
Carnegie  Mellon  University 


Motivation 


Chris  Urmson 
Carnegie  Mellon  University 


GC  Testing  &  Performance 


About  6,000  miles  total 

-  4500  for  Sandstorm 

-  1500  for  Highlander 

Greatest  distance 

-  178  miles  in  desert 

-  200  miles  on  race  track 

Sustained  speed: 

35mph  (13.5mps) 

Peak  speed: 

50  mph 

Challenge: 

-  132miles/7  hours 
— 19  mph 


Chris  Urmson 
Carnegie  Mellon  University 


Driving  Software 


£ 


r 

Sensor 

Pointer  , 

1 

r 

T  iri_ J 

T  in 

1 

Terrain 

.  Evaluator  , 

Terrain 

Extrapolator 


Operates  on  a  nominal  route 

Planning  and  execution  highly 
decoupled 


• 

ult 

iinii^r^ 

Geometric 
Path  Planner 

_ i _ 

Speed 

Planner 


Path  Tracker 


s  Urmson 
Carnegie  Mellon  University 


Conformal  Path  Planning 


— I  Chris  Urmson 

Carnegie  Mellon  University 


Speed  Planning  &  Tracking 


•  Speed  Control 

-  Limit  execution  speed 
based 

•  flat  earth  slip  and  roll 
over  bounds 

•  deceleration  limits  to 
achieve  speed  limits 

•  Path  Tracking 

-  Pure  pursuit  with 
integral  correction 
term 


Chris  Urmson 
Carnegie  Mellon  University 


Beyond  the  Grand  Challenge 


•  GC  speeds  approached  limit 
of  performance  given  sensing 
technology 


Chris  Urmson 
Carnegie  Mellon  University 


The  problem  with  Sensors 


•  Good  measurement  accuracy 

•  Irregular  density  of  measurements 

-  Stereo-vision 

•  Poor  measurement  accuracy 

•  Good  density  of  measurements 


Chris  Urmson 
Carnegie  Mellon  University 


Point  Density:  LIDAR 


K 


- W4 

R 


■■ 

— w 

AR 


Chris  Urmson 
Carnegie  Mellon  University 


A  r  (range  error,  m) 


Range  Error:  Stereo  Vision 


iifAWJiiiinaii 

Parameter 

Value 

Baseline  (b) 

50cm 

Focal  length  (f) 

11mm 

CCD  pixel  size  (p) 

7pm 

Chris  Urmson 
Carnegie  Mellon  University 


stopping 


Legend 

^RB 

Braking 
reaction  time 

1^ 

Coefficient  of 
braking 

g 

Gravity 

Chris  Urmson 
Carnegie  Mellon  University 


Swerving 


swerve  distance 


swerve 

offset 


70  r 


60 


50 


CL) 

U 

£  40 

fU 

4-^ 

U) 

TD 

^  30 


{/) 


20 


10 


swerve  offset  =  10m 


10  15  20 

speed  (m/s) 


25 


Constraints 

include: 

-  kinematic 
curvature 
iimits 

-  tip-over  and 

s  ip  curvature 
iimits 

-  steering  siew 
rate  iimits 

-  reaction  time 


Chris  Urmson 
Carnegie  Meiion  University 


Improving  Performance 


System 

Characteristic 


Value 


Swerving 
reaction  time 


0.1s 


Kinematic 
curvature  limit 


0.2  m- 


Maximum  slew 
^ate 


0.067  m-is-i 


Coefficient  of 
-friction 


0.5 


Gravity 


9.8  m/s^ 


V 


*  2//g 


Ql - ^ ^ ^ - U - ^ ^ - J 

0  5  10  15  20  25  30 


speed  (m/s) 


Chris  Urmson 
Carnegie  Mellon  University 


Space  of  Navigation 
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Chris  Urmson 
Carnegie  Mellon  University 


Chris  Urmson 
Carnegie  Mellon  University 


Control  of  Autonomous  Mobile 
Robots  in  Unknown  Terrain: 
Past  Research,  Future  Challenges 


ii. 


Laura  Ray 


Workshop  on  Mobility  and  Control  in  Challenging  Environments 

Olin  College,  Needham  MA 

October  5-6,  2006  t  h  a  y  e  r  s  c:  h  c>  o  l  of 

ENGINEERING 

AT  DARTMOUTH 


Outline 


•  Cool  Robot  -  lessons  in  design  of  a  high  longevity,  low-cost 
robot 

•  Terrain  diagnostics  and  mobility 

•  Terrain  characteristics  and  high-speed  cooperative  control 

•  Work  in  progress 


Cool  Robot  Design  Overview 


Summer  Deployment  of  Instrument  Networks 

•  15  kg  payload  (or  40  kg  towed),  75  kg  vehicle 

•  500  km  in  2  weeks  (0.4  m/s),  max.  1  m/s 

•  Twin  Otter  transport 

•  Inexpensive  (<  $20k) 

•  Generates -100  -  340  W 

•  GPS  Navigation  and  Smart  Mobility  without  vision 


270  W  electrical  power  @  20  sun  elev. 
35%  is  reflected  power 

Top  19% 


(direct) 


Back  8% 
(refl.  only) 


Front  59% 
(direct  +  refl.) 


Sides  20% 
(refl.  only) 


Terrain  Characteristics 


m  scales 


—  do  not  present  mobility  issues  (good 
traction  &  clearance) 

—  affect  power  consumption,  control  & 
navigation 

—  100’s  km  without  sastrugi 
•Chart  routes  around  large  sastrugi 


Mobility  Design 


Four  wheel,  direct  drive 

—  efficient,  simple,  good  traction 

—  ATV  tires  run  flat,  <  20  kPa  (3  psi) 

Lightweight  construction  (low  resistance)  * 

—  target  75  kg,  actual  60  kg 

—  target  R/W  =  0.24,  actual  R/W  =  0.21  Summit,  Greenland  snow 

Sensors  for  “smart  mobility” 

—  tilt,  sinkage,  gyrocube, 

— motor  current,  panel  power 

—  temperature,  wind  speed/direction 


Smart 


What  can  I  measure  in-situ  to 

•  estimate  maximum  attainable 
speed 

•  estimate  maneuverability 

•  avoid  immobilization 

•  retain/augment  stability 

•  maintain  stability/performance 
of  group  dynamics  (cooperative 
control)  at  high  speed 


Tire-Terrain  Characterization 


from  Vehicle  Performance 


Rigid-Bdy  dynamics  are  well-known 
Tire/track  forces  are  influenced  by  terrain 

Rigid  Terrain  Model: 


~  i^xfl  ^xfr  ^xrl  ^xrr  ) 


m 


y 


X' 


-  -V^r  H - ^Fyfl  +  ^yfr  +  ^yrl  +  ^ 


m 


yrr 


) 


^ xfr  y^xrr  ^'x/l  ^  yfr  ^yfl  V/ 

~  i^yrl  y'yrr  z  res^ 

->  With  appropriate  sensor  suite,  tire 
forces  are  observable. 


Extended  Kalman-Bucy  Filtering 
for  Tire  Force  Estimation 


*  Sensors  -  x  and  y  accelerations,  yaw  rate,  wheel 
speeds,  vehicle  speed,  inputs 

*  Augment  rigid-body  dynamics  with  tire  forces  as 
random  walk  model 

*  Implement  five-step  filter 

•  Propagate  dynamics  state  estimate 

•  Propagate  covariance  ->  covariance  estimate 

•  Compute  filter  gain 

•  Update  state  estimate 

•  Update  covariance 

*  Compute  wheel  slip/slip  angle  from  state  estimate 


Originally  developed  and  tested  for  off-line  estimation 


Estimated  Lateral  Force  vs.  Slip  Angle 
and  Normal  Load  for  Two  Tire  Pressures 


Terrain  Diagnostics 


•  Semi-empirical  theory  (Bekker,  Wong) 
models  motion  in  deformable  terrain 
(1950’s,  60’s) 

•  Shear  stress  and  normal  stress  are 
functions  of  terrain  parameters 

•  Cohesion 

•  Friction  angle 

•  Shear  deformation  modulus 

•  Sinkage  parameter(s) 

•  Stress  distribution  parameters 


•  Traction  and  resistance  depend  on  stress 
distributions 


Dynamic/transient  effects  not  modeled 


Estimation  before  Modeling 


Decouples  force-slip  estimation  from  semi-  ^ 
empirical  model  I 

•  Tests  validity  of  model  with  a  range  of  data/terrain  " 
characteristics 

Real-time  impiementation  provides  force-slip 
characteristics  independent  of  terrain 

•  Gets  to  heart  of  stability  augmentation  and  “peak  seeking” 
control 

•  . .  .but  still  allows  for  identification  of  terrain  via  existing  terrain 
parameters 

•  May  be  possible  to  identify  terrain  based  on  “normal” 
maneuvering  that  is  sufficiently  rich 


Terrain  Diagnostics  and  High- 
Speed  Cooperative  Control 


•  Potential  Function  Approach  -  scalar  gains 
control  applied  force  magnitude 

T(r,r)=-VV{r)  +  d(r,r) 
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Testbed  for  Dynamic  Cooperative 


Control  and  Terrain  Diagnostics 


4WD,  passive  joint 

Comparable  performance  to 
“Dragon  Runner” 

•  speed  ~10  m/s 

•  yaw  rate  ~1  rev/sec 

•  acc  ~  7  m/s^  (hard  surface) 

•  Parts  cost  ~  $5k  per  robot 

Plastic-molded  chassis  w/  “drop- 
in”  components 

M  =  12.4  kg 

Sensors:  GPS,  angular  rates, 
linear  accelerations,  magnetic 
compass,  motor  currents,  wheel 
speed 

7  robots,  wireless  inter-robot 
communication 


Example  Trajectory  -  One  Robot 


Fixed  potential  function  scalar  gains 


Example  Trajectories  for  High 
and  Low  Adhesion  Surfaces 


Fixed  potential  function  scalar  gains 
w/  local  slip  setpoint  control 


G 


High  adhesion 


D 


lower  adhesion  no  slip  control  lower  adhesion,  slip  control 


Very  low  adhesion  no  slip  control 


Very  low  adhesion,  slip  control 


Present  and  Future  Research 


•  Evaluation  of  terrain  diagnostics 

•  Estimation/inference  of  terrain  parameters 
from  tire  force  estimates 

•  Scouting? 

•  Design  light-weight  inexpensive  “scout”  vehicle  for 
terrain  estimation,  extrapolate  to  heavy  vehicle 

•  High-speed  cooperative  controi 

•  Distributed  estimation/sharing  terrain  information 

•  Interplay  between  communication,  latencies, 
terrain  variation,  and  control  performance 
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Active  Sensing  of  Terrain  by  a  Crawling 
Robot:  Optimizing  Gait  for  Terrain 

Selectivity 


Richard  Voyles 
University  of  Denver 
Amy  Larson 
University  of  Minnesota 


Motivation:  Small  Resource- 
Constrained  Robots  m 


Lebanon,  IN 
IN-TFl 
Aug.  2003 
NSFR4 


•  NSF  SSR-RC 

-USF 

-UMN 

•  Medium-term 
Research 

•  Near-term 
Fieldability 


Lakehw 
NJ-I 
Feb.  2 
NSF  R4/S 
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Example:  CRAWLER  to 
Augment  Core-Bored  Seareh 


3.  Search  Occluded  Spaces  with  Tethered 
Robot  Dropped  Through  Bore  Hole 


1.  Bore  Hole 


W 


2.  Search  Void  with  Camera 


SEARCH 


SYSTEMS 


CRAWLER  crawling 
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Objectives 


•  Small  size  needed  for  access  tends 
to  limit  capability 

-  Power,  sensors,  actuation,  computation 

•  Adapt  resources  to  compensate 

-  Physical  adaptation 

-  Control  adaptation 
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CRAWLER 

a.k.a.  TerminatorBot 


•  Two  3-DoF  Arms  that  Stow 
Inside  Body 

•  Dual-Use  Arms  for  both 
Locomotion  and  Manipulation 

•  Four  Locomotion  Gait  Classes: 

-  “Swimming”  Gaits  (dry  land) 

-  Narrow  Passage  Gait  (no  wider 
than  body) 

-  “Bumpy  Wheel”  Rolling  Gait 

-  “Body-Roll”  Dynamic  Gait 


University  of  Denver  Department  of  Engineering 


ThrowBot  -  Gross/Fine  Locomotion 


Stowed  Configuration  Deployed  Configuration 


Barrel  launching  originated  from  DARPA  Distributed  Robotics  Program. 
Not  actually  hardened  for  throwing. 


University  of  Denver  Department  of  Engineering 


Novel  Multi- Axis  Force  Sensors  for 
Soil  Probing 


•  Ks  =  4Ea3b(R2+Rr+r2) 

3(R-r)3 

•  8  =  3(R-r)(5R+r)Ts 

16Ea2b(R2+Rr+r2) 

•  ^  =  yIs 

En  Fr 

•  y  =  3(5R+r)((R-r)2+a2) 

8a(R-r)  (R2+Rr+r2) 

based  on  Vischer  &  Khatib,  1990 

University  of  Denver  Department  of  Engineering 


The  Gait  Bounce  Terrain  Metric 


•  Measure?: 
-  Tilt/Accel 
-Vision 
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Robot’s  Eye  View 


Homing  by  Visual  Servoing 


•  Visual 
Servoing 

-  2-D  Sensor 

-  1-D  Problem 

•  Can  we  make 
use  of  the 
extra  info?? 


<4  « 

*  ^ 
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Gait  Bounce  Signatures  and 
Alternatives 

other  Work 

•  Looking  Ahead  (Vision) 

-  Aerial  and  Elevation  Maps  (correspondence 
problem)  -  Gennery  (1989),  Kweon  &  Kanade 
(1992),  Huber  &  Hebert  (1999),  ... 

-  Elevation  Maps  -  Langer  et  al.  (1994), 
Simmons  et  al.  (1995),  Gennery  (1999),  ... 

-  Scene  Analysis  -  Seraji  and  Howard  (2000), 
Talukhder  et  al.  (2002),  Huber  et  al.  (1998),  ... 

•  Vehicle-Terrain  Sensing 

-  Wheels:  Bekker  (1969),  lagnemma  et  al. 
(2001),  Yoshida  &  Hamano  (2002),  lagnemma 
et  al.  (2003),  ... 

-  Limbs:  Hirose  (1984),  Espenschied  et  al. 
(1996),  Wettergreen  et  al.  (1995),  Lewis  & 

Gait  bounce  signature  from  various  terrains.  Bekey  (2002),  Kurazume  &  Zhang  (1996), 

Raibert  (1984),  ... 
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Bounce  Normalization 


•  Compensate  for  Body 
Roll  Assuming  Fixed 
Features 

•  Compensate  for 
Perspective  Distortion 
Assuming  Linear 
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Terrain  Classification 
Using  Spatial  Discriminants 
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Experimental  Results  -  Raw  Classifiers 

90 
80 
70 
60 
50 
40 
30 
20 
10 
0 

Raw  FFTall  FFTsec 
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Need  for  Adaptation 


Carpet 


Foam 


Energy  (J) 


Swim  1 

63.6 
62.4 

63.7 


Swim  2 
71.9 
75.4 


Efficiency  (J/cm) 


Swim  1 

Swim  2 

Carpet 

3.70 

1.43 

BBS 

2.12 

3.57 

Foam 

1.76 

1.59 
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Active  Sensing:  Spatiotemporal  Patterns  and  Gaits 


Swiiiiiiiiii^  Gait  Class 
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PositionArm 


Observation  Sequence 


/:0^F 

preprocess 

preprocess 

preprocess 

- 1 - 

QDA 

_ I _ 

classify 

T 

classify 

_ I _ 

classify 

- 1 - 

y 

▼ 

^1 

. 

^2 

O9 
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Experimental  Results 
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General  Applicability?? 


Treaded  Vehicle 


0  0 
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Evolving  Gaits  for  Selectivity 

•  Adapting  the  gait  based  on  iocai 
terrain  improves  vehicie  performance 

•  Current  terrain  ciassification  reiies 
on  variabiiity  in  gait  bounce  across 
terrains. 

•  Can  we  evoive  gaits  to  maximize 
variabiiity  (thus  seiectivity)? 


University  of  Denver  Department  of  Engineering 


Genetic  Algorithm 


■•■Encoding  gaits  (individuais). 

-  Matrix  of  joint  motion  (1st  row  is  initiai 
position). 

•  Cuiiing  Agents 

-  Remove  unattainabie  Joint  positions 

•  Limb/Terrain  interaction  Modei 

-  Estimate  gait  bounce 

•  Objective  (Fitness)  Functions 

-  Guide  genetic  seiection 

University  of  Denver  Department  of  Engineering 


Culling  Agents 

Gait  Trajectory  = 


Culling  Conditions 

-  Fingertip  or  eibow  inside  the  body 

-  Arms  crossed 

-  Eibow  motion  moves  forearm  thru  upperarm 

-  Joint  iimits  exceeded 

-  Any  portion  of  forearm  inside  body  (not  yet 
implemented) 

University  of  Denver  Department  of  Engineering 


Limb/T errain  Interaction  Model 


Bekker's  Wheel/Terrain  Interaction  Model 


Estimates  wheel  sinkage  (s) 
Uses  load  (W)  on  wheel 
Uses  soil-specific  coefficiet^ 

-  kg :  cohesion 

-  k  :  friction 

-  n  :  exponent 


[nterpreting  TerminatorBot's 
fingertip  as  a  wheel 


S  =  [  3W  /  (3-n)(k^  +  bk^)sqrt(D)  ]  *  {2/(2n+1 )) 

University  of  Denver  Department  of  Engineering 


Objective  Functions 

Maximize  Distance 

^  ”  ^(vias)  ^i-1  (  YflJ  "  YflJ-l  ) 

aj  =  1  when  limb  in  contact  with  ground. 

Yft  j :  y-coord  of  fingertip  at  via  / 

Maximize  Efficiency 

E  =  Z(vias)  iaj+— iaj.-|)'(6j  -  6i.i)'©n 

e„ :  energy/radian  when  not  in  contact  with  the  ground 
e^  :  energy/radian  when  in  contact  with  ground 

Maximize  Selectivity 

Ab  =  sqrt  [  I,freq)  ( ^  ] 

tn :  terrain  n 

bfn :  bounce  signature  from  traversing  terrain  t„ 
(obtained  from  limb/terrain  model) 

University  of  Denver  Department  of  Engineering 


Evolved  Gaits  - 
Fitness  Metrics 

•  Efficiency 

•  Selectivity  Only 

•  Distance  &  Selectivity 
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Clustering  Results:  Gaits  on  Terrains 


•  Simulated,  Pre-processed  Gait 
Bounce  Signature  (“cluster  space”) 


Selectivity  Fitness 


H  fitness 
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Summary 


•  Terrain  Classification  from  V  Servo  Error 

-  Gait  Bounce  metric 

-  No  Additional  Sensors 

— 90%  recognition  accuracy  over  5  terrain 
samples  (~700  trials) 

-  “Somewhat”  applicable  to  general  vehicles 

•  Preliminary  work  on  gait  evolution  for 
classification  selectivity 


University  of  Denver  Department  of  Engineering 
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Core-Bore  CRAWLER  Video 


w 


Wheel 
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Limb 


Energy  Efficient  Reconnaissance 
using  a  Rotational  Legged 
Locomotion  Platform 


Damian  M.  Lyons 
Robotics  &  Computer  Vision  Lab 
Ford  ham  University 
NY  1 0458 

dlyons@cis.fordham.edu 


ARL  Workshop  on  Mobility,  Needham  MA,  Oct  5*^*^  2006. 


Rotational  Legged  Locomotion* 


*  Funded  by  ARL  STIR  Grant  P-4941 1-CI-II 

□  Reconnaissance  task 

□  Legs  versus  wheels 

□  Concept:  Rotational  Legged  Locomotion 

□  Platform  description 

□  Analysis  of  natural  motion 

□  Motion  Strategies 

□  Energy  Efficiency 

□  Summary,  Conclusions,  Next  Steps 
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Reconnaissance  task 


□  Traverse  a  designated  area  with  the 
objective  of  recording  and  reporting 
terrain  and  target  features. 

□  Will  encounter  a  wide  range  of  terrain 
types 

□  May  need  to  operate  for  long  periods 

□  May  need  to  evade  pursuit 

c^Energy  efficient,  versatile  locomotion 


October,  2006 
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Legs  versus  Wheels 


□  Legs  -  versatile: 

■  Can  step  over  obstacles  and  into 
depressions 

■  Need  to  lift  as  well  as  propel 

□  Wheels  -  efficient: 

■  Need  smoother  terrain 

■  Don't  need  to  lift,  just  propel 
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Rotopod  Concept: 

R^otational  Legged  Locomotion 


□Vertical  wheel  analog 

e.g.,  [Altendorfer  et  al.  2001] 
[Quinn  et  al.  2001] 


Virtual’  wheel 


body 


////// 


□Horizontal  wheel  analog 


Leg 

=spoke 


Virtual’  wheel 
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Design  Approach 


□  Exploit  the  natural  modes  of  motion 
of  the  mechanism 


October,  2006 
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Platform  Description 


The  E-otopod  Mechanism 


Rotopod.  (a)  Plan  View  (b)  Side 


Open 
Dynamics 
Engine  (ODE) 
Simulation 


7 


October,  2006 


Locomotion 


□  Platform  walks  by  rotating  around  leg 
endpoints 


Slow  Fast 


□  Legs  can  'step'  over  obstacle  and 
depressions 
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Robot  Platforms 
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Natural  Motion:  Overview 


□  Overall  trajectory 
is  cycloidal 


□  Rotation  around 
leg  causes  'loop' 

Reaction  mass  pulls  and  lifts 
as  it  approaches  opposition, 
then  pulls  the  other  way 
as  it  leaves  opposition 


Position  of  Platform  Center 


o 

>- 


X  ordinate 


Q  4 

0  2 

^ - - 

.4  -0.2  Q ,  ( 

oo 

o 

CD 

<3 

0  6 

n  R 
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Leg 

endpoint 

rotation 


Resultant 
Motion  of 
Center 
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Natural  Motion:  Hypotrochoid 


□  Prolate  Hypotrochoid 

/  \  ^ 

(has  loops)  (on  the  inside)  (surface  of  a  circle) 


y 

X 


f  —  d  \ 

(c?  -  sin  ^  -  A  sin  - t  , 

\  b  J 


{a-  b)  cos  t  +  h  cos 
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Natural  Motion:  Parameters 


d {6)  -  ll^SinpSin  ^ 


Path  of  center 


4  =leg  length 
P  =  leg  sep.  angle 
a  =  max  tilt  angle 


=a-{b  +  h) 

Ijib  -d{6)  -  ll^SinpSin^ 
a-(b-h)  =  l^SinP  -l^Sin{P  +  a) 
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N0tural  Motion!  Forces  acting  to  tilt 
platform  and  raise  leg 


1 .  Single  leg  raise 

Fj.=mj,oo2r 


2.  Two  leg  raise 
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Natural  motion  of  platform: 

Oscillatory  Motion  of  Leg 


□  Rotating  reaction  mass  causes  leg  to 
rise. 

□  Gravity  causes  the  leg  to  fall. 

□  Three  coupled  oscillators:  one  per  leg. 

k^z.  +  k2Z.  =  F.Sin0 

z-  is  the  z  ordinate  of  the  ith  leg 
kj,  k2  constants  of  platform  masses  and  lengths 
F-  total  force  acting  to  raise  leg  i 
0  reaction  mass  angle 
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Natural  motion  of  platform: 

bI®  Oscillatory  motion  of  leg 


9;=n/ 3 


Leg  height  versus  rotation  mass  arm  angie 
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Open  Dynamics  Engine  (ODE)  simulation  results 
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Natural  motion  of  platform: 

Phases  of  leg  motion 


□  Leg  height  has  three  phases: 

□  Resting  (1) 

□  Slipping  (2) 

□  Stepping  (3) 


1  2  3 
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Natural  motion  of  platform: 

bI®  Three  Coupled  Oscillators 


Leg  Height  versus  rotation  mass  arm  angie 


Rotation  mass  arm  angle 


Open  Dynamics  Engine  (ODE)  simulation  results 


17 


October,  2006 


Natural  motion  of  platform: 
it®  stability 


□  System  will  topple  if  a  leg  is  raised 
beyond  a  critical  angle  =  leg  angle  p. 


T{a)  =  fXSinifi  -d)-f^  {e)lfiosP 
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a, 


Natural  Motion  of  platform: 
Slii  rnsxirTTium  tilt  angle 


Leg  i  height  z,  peaks  when  9  =  9;-n/2 
T  {a)  =  fJ^Sin{/5  -a)- {6)l^Cos/3  =  mpd 


0,+- 


a  = 


— j  ^T{a)de 


p  0,-1 


2 


^[Sin(fi)f^-42Cos(J3)F^] 


1 


n2 


2co 


Unstable  if  a*  >  p 
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Natural  Motion  of  Platform: 
Energy  stored  in  leg 


October,  2006 
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a, 


Natural  Motion  of  platform: 
i®ii  galloping  instability 


If  leg  /  height  z,  peaks  at  65  -  e  Oi-k/I 
For  stability,  z,  must  go  to  0  by  ^  =  6*  +71/2  ? 


\W)de 


.  1 
a  >  — 

2 


/, 


2m 


{Sin{/3-Q.5a)f^  +  l] 


CO 
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Motion  Strategies: 
Leg  Lengths 


□  Modify  leg  lengths  to  change  motion, 
will  rotate  around  platform  center. 


■Qt92- 


Slow,  negative  rotation  around  center. 
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Motion  Strategies: 

Leg  lengths  and  Rmass  Velocity 


□  Rotate  around  leg  endpoint 


n  QK 


Faster,  positive  rotation  around 
one  or  more  Leg  endpoint 
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Motion  Strategies: 

R^^sults  of  experimentation 


□  Varying  leg  selection  &  length 


Extension=0.4 


Extension=0.6 


1  leg  extended 
Varying  amounts 


Extension=0.8 
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Motion  Strategies 
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Motion  Strategies: 

Results  of  experimentation 


□  2  legs  extended 


October,  2006 


26 


Motion  Strategies: 
Cycloid  Gait 


□  Net  forward  motion  given  by  difference  of 
radii 


□  Easy  to  make  corners  -  precision  given  by 
smaller  radius 

□  Thickness  of  path  covered  given  by  sum  of 
radii 

□  Recoverage  of  area  given  by  ratio  of  radii 


October,  2006 
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Motion  Strategies: 
Cycloid  Gait 


October,  2006 
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Motion  Strategies: 

Miiiii  Cycloid  gait  path  planning  example 
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Motion  Strategies: 

Miiiii  Other  compound  strategies 


Area  coverage: 

(gl=[(LlA00)(L2A00)h 
g2 =[(L2, 100)(L3,100)])x4 


Straight  line 
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Energy  Efficiency 


□  Specific  Resistance 
Natural  Motion 


Specific  Resistance 

power 


s  = 


weight  X  velocity 


ODE  Simulation: 
s  =0.03  with 
Center  velocity  1.29  m/s 
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Energy  Efficiency 


□  Limiting  Reaction  Mass  Saturation  Torque 


.5  -0.5  fjm 

L  0 

^  /  /(Tv 

Payload  versus  radius 
(total  wgt  =  payload+20kg) 


■  Sat=15Nm 
Sat=20Nm 

■  Sat=30Nm 
Sat=50Nm 


1.00%  5.00%  10.00%  15.00%  20.00% 

Payload  increase  as  %  of  total  weight) 


25.00% 


T (a)  =  fJ,Sin{fi -a)- f  {0)1  Cos /3 
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PIstform:  Second  Prototype 


Reaction 

Mass 


Controller 

Box 


Payload 

Platform 


Power 
Umbilical 
(for  testing 
only) 


Reaction 
Mass  Arm 


Reaction 

Mass  Motor 

Linear 

Actuator 

Motor 


Linear 

Actuator 
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»  Summary 


□  Summary 

■  Platforms  Built: 

□  Kinematic  simulation  (in  python) 

□  ODE  simulation  (in  c) 

□  Second  prototype 

■  Feedforward  motion  strategies 

□  Motion  along  a  curve 

□  Compound  gaits,  esp.  cycloid 

■  Energy  efficiency 

□  Low  energy  natural  motion 

□  Effect  of  payload  on  energy  use 
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m  Next  Steps 


□  Continuous  motion  strategies 

■  Rolling 

□  Stepping  over  obstacles 

■  Controlling,  selecting  footfalls 


□  Search  patterns  and  metrics 

■  Energy-efficiency  &  coverage 

□  Reconnaissance  sensor  deployment 

■  Rotating  camera,  laser  scanners 
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Thank  You 
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Joseph  Ayers 
Departmenr  of  Biology  and 
Marine  Science  Center 
Northeastern  University 

East  Point 

Nahant,  Massachusetts 


Biomimetic  Performance  Advantage: 

Omnidirectional  Walking 
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Northeastern 


Undulatory  Vehicle 


Neuronal  Circuit-Based  Controller 

Based  on  Command  Neuron,  Coordinating  Neuron,  Central  Pattern  Generator  Model 


CN:  Command  Neurons 
CoN:  Coordinating  Neurons 
CPG:  Central  Pattern  Generator 


Lobster 


Neuronal 

Oscillator 


Pattern 

Generator 


CPG 


Flexor  Extensor 


iNeuronai 

Oscillator 


Flexor 


Invertebrate  C?&s 


K  I  _  I  _ •  ■ _ 


20  ml/  20  mV  20  mV  20  mV 


Dynamical  Neuronal  Models 

When  synoptical ly  isolated, 
lobster  LP  neurons  exhibit  only 
chaotic  regimens  of  bursting 


Internal  Calcium  buffering 
modulates  chaos 


Local  False  Nearest 
Neighbor  Analysis 

Lobster  neurons  have  only  4 
degrees  of  dynamical  freedom! 


Dimension 
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Electronic  Neurons  and  Synapses 


:  L 
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1 
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Hindmarsh  Rose  Equations 

"  ''■ 

Membrane  Potential 

=  e  -  fx\i)  -  y(t)  -gw(t) 


dz(t) 

dt 

dw(t) 

“dT 


=  e  -  fx^(t)  -  y(t)  -gw(t) 

Fast  Conductances 
=  m(-z(t)  +  S(x(t)  +  h)) 
Slow  Conductances 
=  n(-kw(t)  +  r(y(t)  +  I)) 

Calcium  Dynamics 


Institute  for  Nonlinear  Science:  UCSD 

Pacemaker  Prototype  Chaotic  Prototype 


Institute  for  Non-Linear'Science 
UC  San  Diego 
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Mobile  Robotics  for  in  vivo 
Surgical  and  Battlefield  Applications 
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-  Minimally  invasive  surgery  (MIS) 
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Abdominal  In  vivo  Robots 
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•  ~90%  of  battlefield  deaths  take  place  within  30  minutes 
of  the  initial  injury 

•  ~  50%  of  these  deaths  are  due  to  hemorrhaging  in  the 
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Mobile  Robot  Platform 


•  2  independent  wheels 

-  2  electric  motors 

-  Forward,  reverse,  turning 

-  Tail  to  prevent  spinning 

•  12-1 5mm  diameter 

•  Tethered  for  power,  or 
wireless 

•  Camera 

•  Biopsy 

•  Sensors 


I  AT. 
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In  Vivo  Mobility  Challenges 


In  vivo  environment: 

-  Deformable 

-  Slick 

-  Hilly 

Too  little  traction 
Too  much  traction 
High  centering 

Modeling 
Wheel  testing 


Mobility  Challenges 
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Experimental  Platform 


Linear  slide 
Induce  slip 
Adjust  normal  force 
Measure  drawbar  force 


slide 


SR  =  l- 


X 


cm 


rO 


cm 


Test  complex  geometries 


hinge 
load  cell 

,  drive  motor 
^drive  belt 
test  wheel 
liver 


Experimental  Platform 
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Profiles  Tested 

•  Smooth 

•  Female 

•  Male 

•  Helical 

•  Brush 


smooth 


female 


male 


Wheel  Profiles 


10/06/2006 

ROBOTICS  AND  MECHATRQNICS  LAB 

9/28 

UNIVERSITY  OF  NEBRASKA  -  LINCOLN 

Experimental  Results 


Helical  wheel  design 


SR  =  l- 


X 


cm 


rO 


cm 


o 


0.1 

0.08 

0.06 

0.04 

0.02 


TO 

i  0 

Q 

-0.02 

-0.04 

-0.06 


i.. . 

I 


0.45  N 


0.05 


0.1  0.15 

Slip  Ratio 


0.2 
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NeBiasKa  Wheel  Test  Results 


•  Reduce  motion  resistance 

-  Larger  diameter,  less  ground  pressure 

-  Less  ground  pressure,  less  sinkage,  less  torque  loss 

•  Minimize  fluid  effects 

-  Good  tread  design 

-  Avoids  hydroplaning 

•  Helical  profile  is  superior 
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Finite  Element  Analysis 


•  Loads  -  vary  the  normal  forces  (weight) 

•  Motions  -  translation  and  rotation 

•  Results  -  force  transducers  measure  drawbar  force 

•  Wheel  is  rigid 

•  Tissue  is  liver  material  model  (SLS  model) 
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Tissue  Damage 


•  Routine  grasping  forces  of  40  N 


-  Corresponds  to  pressures  of  ~  400  kPa 


•  Finite  element  model  shows  max  stresses  of  1 .95  kPa 

HELICAL  WHEEL  ON  LIVER 
Time  =  0 


Finite  Element  Modeling 
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JNeDiasle  Drawbar  Forces 


•  Four  slip  ratios,  three  weights 

•  Results  compare  favorably  to  lab  data 

-  Approximately  same  magnitudes 

-  Same  weight  trend 

-  Same  slip  ratio  trend 
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JNeDiastia  Geometry  Analysis 


•  Larger  diameter  is  better 

-  less  motion  resistance 

•  Lower  pitch  angle  is  better 

-  high  stress  concentrations 

-  2  treads  ->  smooth  velocity  profile 


jr 

-  -  I  ) 

m 


Thinner  tread  is  better 

-  higher  stress  concentrations 

Larger  tread  depth  is  better 

-  Up  to  a  point  "  1 .5  mm  depth 
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NelSla  Redesigned  Mobile  Robot 


.4. 


m 


-  ~  ca^.) 
w 


Traverse  entire  abdominal  cavity 

-  No  tissue  damage 

-  Used  for  exploration 


Crawler 
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Mobile  Camera  Robot 


Exploring  abdominal  cavity 
2  port  cholecystectomy  possible 
Adjustable-focus  camera 
Camera  angles  from  any  point 


Mobile  camera  robot  cholecystectomy  View  from  laparoscope 
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Biopsy  Robot  Design 


Camera  slots 
Adjustable-focus  camera 
Novel  mechanism 


Camera 
Field  of 
View 


Mechanism 

Housing 


Slider 
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Mobile  Biopsy  Robot 


Sample  tissue 
Clamp  artery 
Apply  large  force 


\-’0 . 

- r-i.,- - j 

•  / 

^  , 

F  cable 

Biopsy  Mechanism  Design 
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In  vivo  Testing 


•  Mobility  on  abdominal  organs 

•  Sampled  liver 

•  Retrieved  sample  after  extraction 

•  Demonstrated  a  one  port  procedure 


Laparoscope  view  Mobile  biopsy  robot  view 
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Current  Efforts 


•  Mobile  Wireless  Camera/Biopsy  Robot 

•  NOTES  robot 

•  Mobility  in  blood  filled  cavities 


MRC  Test 
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NOTES  Progress 


•  Natural  Orifice  Transgastric  Endoscopic  Surgery  (NOTES) 

•  Incision-less  surgery 

•  Multiple  robots 


Mobile  NOTES 
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Ceiling  Pan/Tilt  Camera  Robot 
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NOTES  Robot 

Stereo  camera  pair 


2  degree  of  freedom 
“shoulder”  joint 


1  degree  of  freedom 
“elbow”  joint 


Forceps 
Cautery 
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Natural  Orifice  Surgery 


1 ' 
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Insulflated 

Abdominal 

Cavity: 


Robot  during 
insertion 


Incision  in 
stomach  wall 


Magnetic  handle 
outside  the  patient 

/  ■ 

In  VIVO 

robot 


Electrical 

power/signal 

tether 
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Use  of  a  NOTES  Robot 

Handle 


Section  of 
Abdominal 
Wall 


Magnets 

inside 

robot 


Inside 

Patient 


Magnets 
embedded 
in  handle 


Outside 

Patient 


Electrical 
Power/Signal 
Tether 


In  vivo 
Robot 
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The  Future 


•  Battlefield  ops  with  portable  imaging,  path  planning, 
automated/programmed  surgery 

-  Insert  this  into  a  heavily  bleeding  wound  and  have  the  robot 
seek  out  the  source  of  the  bleed  and  stop  it. 

•  SLAM  -  Simultaneous  Localization  And  Mapping 

-  Include  other  modalities  (CT,  MRI,  Ultrasound,  X-ray) 

•  Depth  and  range  mapping  from  stereovision 

•  Path  and  surgical  planning  using  CT  and  MRI 

•  Semi-automated  and  automated  processes 
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Questions 


http://robots.  uni.  edu 
http://www.  unmc.  edu/mis 
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Symbolic  Motion  Planning  for 
Highly  Maneuverable  Robots 


Emilio  Frazzoli 
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Massachusetts  Institute  of  Technology 
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Mobility  and  Control  in  Challenging  Environments 
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Aerospace  Robotics  and  Embedded  Systems  Laboratory 


Motivation 


MIT  Acrobatic  Helicopter  (01)  UCLA  Golem  2  (DGC  05)  UCLA  UAV  Fleet  (  06) 


•  Allow  autonomous  vehicles  to  push  the  boundaries  of  their  operational 

envelope:  fly/drive  fast,  react  quickly  to  external  events,  etc. 

-  critical  ability  for  robotic  vehicles  and  UAVs  in  uncertain/dangerous/hostile 
environments. 

•  Beyond  the  capabilities  of  “traditional”  control  design  techniques. 

-  New  modeling/design  paradigms  needed. 

•  General  applicability: 

-  Aircraft,  Spacecraft,  Ground  robots.  Sailboats,  Swimming  robots  etc. 
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The  basic  intuition 


Human  pilots  fly  acrobatics  combining  well-practiced  “maneuvers,”  or 
elementary  behaviors. 

Can  we  build  a  mathematically  sound  framework  for  motion  planning 
and  control  based  on  this  idea? 
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Hierarchical  decomposition  1/2 


•  A  hierarchical  structure  is  desired,  hiding  (by 
“abstraction”)  unnecessary  details  at  the 
planning  level. 


•  The  common  approach: 

-  Choose  a  priori  a  simplified,  convenient, 
model  of  the  system  dynamics  for  the 
higher  layers  and  force  it  upon  the  lower 
(control)  layer,  (e.g.,  discrete  modes, 
kinematic  models,  piece-wise  polynomial 
paths,  etc.) 


•  Guarantees  (safety,  stability,  performance)  on 
the  behavior  of  the  system  do  not  transfer 
from  one  level  the  others. 
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Hierarchical  decomposition  1/2 


•  A  new  approach: 

-  Choose  a  subset  of  actual  trajectories  of  the 
system,  and  allow  motion  planning  only  as  a 
combination  of  such  motion  primitives,  or 
closed-loop  behaviors. 


•  Pro:  consistent  hierarchical  system.  Any 
command  from  the  planned  can  be  executed  by 
the  controller  (is  a  “natural  trajectory”) 

•  Pro:  model-free,  no  need  to  know  the  differential 
equations  describing  the  dynamics. 

•  Con:  over-constraining  of  trajectories.  Only  allow 
behaviors  which  can  be  generated  through  the 
sequential  combination  of  known  primitives. 
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Planner 


r- 


Motion 

primitives 


Problem  formulation 


Consider  a  time-invariant  dynamical  control  system  <S: 


x{t)  =  f{x{t),u{t)),  X  G  A! ,u  G  U  G  (1) 


and  its  flow  under  a  (possibly  closed-loop)  control  law:  /i :  [0,tf]  x 
U 


x{t)  =  (/?^(x(0),t) 


Given  an  initial  condition  xq  and  a  target  Xf,  find  a  (piecewise  continu¬ 
ous)  control  law  p  such  that: 

1.  3tf>0\Xf  =  (p^(xo,tf),  /dynamics7;- 

2.  C{x{t),  pit,  x{t)))  <  0,  Vt  G  [0,  tf\  ^operational  envelopey;. 

3.  J{x,  u)  =  j(x)  is  minimized  ^performance  criteriony. 
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Symmetry 


•  The  complexity  of  the  motion  planning  problem  in  general  is  daunt¬ 
ing. 

•  Exploit  geometric  structure  reduce  the  problem  to  a  form 
of  kinematic  inversion  (without  introducing  simplifications  in  the 
model.) 

•  Symmetry,  i.e.,  invariance  with  respect  to  a  class  of  transformations 
on  the  state,  is  a  fundamental  geometric  property  of  many  sys¬ 
tems  of  interest,  e.g.  mobile  robots  and  autonomous  vehicles. 
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Motion  Primitives 


<P,  {Xo,t) 


%  (x„) 


•  Motion  primitive:  equivalence  class  of  finite-time  integral  curves  of 
(1)t  G  [0, r]  I — ^  modulo: 

-  Time  translations 

-  Actions  of  Q. 


•  Note: 


-  If  F{x,  u)  =  x),u),  feasibility  with  respect  to  operational 

envelope  constraint  is  uniform  on  motion  primitives. 

-  If  7(x,  u)  =  7(^(5',  x),u),  the  cost  of  a  motion  primitive  is  uniform 
on  motion  primitives  (e.g.  minimum-time,  -length,  -energy). 


Aerospace  Robotics  and  Embedded  Systems  Laboratory 


Operations  on  primitives 


•  Prefix,  suffix:  cut  a  mo¬ 
tion  primitive  into  two  pieces. 
Each  piece  is  still  a  motion 
primitive. 

•  Concatenation:  join  two 

motion  primitives.  This  oper¬ 
ation  is  possible  only  under 
certain  compatibility  condi¬ 
tions. 

•  Repeatable  primitive:  A 

motion  primitive  which  can 
be  concatenated  with  itself. 


o 


prefix 


suffix 


sequential  combination 


not  sequentially  combinable 
(assuming  Q  =  ) 


non  repeatable  repeatable 
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Finite  vs.  finite-description  libraries 


•  A  finite  collection  of  primitives  leads  to  a  discrete  reachable  set  (e.g., 
a  lattice). 

•  Even  though  the  lattice  can  be  made  arbitrarily  dense,  the  length  of 
motion  plans  may  become  unbounded. 

•  Look  for  families  of  continuously-parameterized  primitives:  maintain 
a  ’’finite  description,”  while  effectively  considering  an  uncountable 
number  of  primitives. 
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Classification  of  Primitives 


•  Trim  Primitive:  a  non-trivial  repeatable  motion  primitive  whose  pre¬ 
fixes  and  suffixes  are  repeatable. 

-  Lemma:  The  closure  of  a  trim  primitive  under  prefix,  suffix 
and  concatenation  is  a  connected  one-parameter  semigroup  with 
identity. 

-  Theorem:  A  motion  primitive  a  is  a  trim  primitive  if  and  only  if  it 
can  be  written  as  a{t)  =  (T^(exp(^«t),  x«),  with  G  g. 

•  A  trim  primitive  is  a  steady-state  trajectory.  The  nature  of  possible 
trim  primitives  depends  on  the  symmetry  group: 

-  Sailboats  x  0(1))  :  straight  lines; 

-  Car-like  robots  {Q  =  SE{2))\  arcs  of  circles; 

-Aircraft-like  robots  {Q  =  SE{2)  x  S^)\  arcs  of  helices  with  a 
vertical  axis. 
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Classification  of  Primitives  2/2 


•  Maneuver:  a  non-trivial  motion  primitive  which  can  be  concatenated, 
from  the  left  and  from  the  right,  with  a  trim  primitive. 

•  Formal  definition  of  “maneuver:” 

-  Well-defined  pre-  and  post-conditions; 

-  A  common  interface  for  concatenation. 
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A  formal  language  for  motion  description 


•  S:  An  alphabet  composed  of  a  finite  number  of  maneuvers. 

•  uj\  words  of  the  language  L(MA)  c  S*. 

•  The  language  Z/(MA)  is  the  set  of  all  strings  accepted  by  a  finite-state 
machine,  called  a  Maneuver  Automaton.  MA  =  {Q,  S,  5,  F} 

-  Q'.  a  set  of  states.  In  our  case,  a  set  of  trim  primitives. 

-  S:  the  already-defined  alphabet. 

-  5  :  Q  X  T,  ^  Q-.  a  transition  function. 

-  qo,  F:  resp.  an  initial  state  and  a  set  of  final,  or  accepting  states. 


c 


Motion  Planning 


•  A  Maneuver  Sequence  is  a  word  uo  e  L{MA),  i.e.,  a  path  on  the 
Maneuver  Automaton  directed  graph.  It  corresponds  to  the  primitive 

u  =  7ri7r2 . . . 

•  A  Motion  Plan  is  a  pair  (cj,  r),  where  r  is  a  vector  of  +  1  non¬ 
negative  coasting  times,  corresponding  to  a  primitive  of  the  form 


LUt  —  <^i{Ti)T:ia2{T‘2)TT2  •  •  •  '^N(^N+i{^N+i)  ■ 
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Kinematic  reduction 


•  Given  an  initial  condition  xq  =  x^J,  the  final  state  after  a  motion 
plan  {uj,  r)  is  xj  =  ^(p/,  Xa^^J,  where: 


■  N 

i=l 


^Wi^aN+i'^N+l) 


gog^  exp(77iri) . . .  exp(77iv+iriv+i) 


•  The  expression  above  has  the  structure  of  a  (forward)  kinematic  map. 


•  Motion  planning  problems  for  complicated  dynamical  systems  can  be 
solved  through  kinematic  inversion! 
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(Sub-)  Optimal  Motion  Planning 


•  An  approximation  to  the  optimal  motion  planning  problem  can  be  ob¬ 
tained  efficiently  by  restricting  allowable  motions  to  the  concatenation 
of  known  primitives. 


Using  the  MA  language,  the  optimal  motion  planning  problem  is  re¬ 
cast  as: 

N{uj) 

{u;,ry  =  argmin  ^ 

i=l 

s-t-:  g f 

r  >  0. 


(5) 


•  Hierarchical  motion  planning: 

-  Combinatorial  component:  Choice  of  maneuver  sequence  uj 

-  Kinematic  inversion  to  compute  coasting  times  r. 
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Example:  Aerobatic  helicopter 


•  Application  of  the  proposed  motion  planning  methodology  to  a  real¬ 
istic  model  of  an  X-Cell  .60  SE  small-size  helicopter. 

•  The  helicopter  is  equipped  with  an  on-board  CPU  and  a  full  avionics 
suite,  including  solid-state  angular  rate  sensors  and  accelerometers, 
GPS  unit,  compass  and  air  data  system. 

•  The  helicopter  dynamics  have  been  modeled  using  a  combination  of 
first-principle  modelling  and  system  identification 


Methodology 


•  Possible  approaches  to  the  design  of  a  motion  library: 

-  model-based  optimal  control  design; 

-  analysis  of  human-piloted  flight  data; 

-  analysis  of  closed-loop  behavior  using  “simple”  feedback  con¬ 


trollers 


•  For  the  sake  of  clarity,  we  will  consider  a  very  small  library  of  motion 
primitives,  containing  only  four  trim  primitives,  and  seven  maneuvers; 

•  In  practical  application,  the  choice  of  the  number  of  motion  primi¬ 
tives  to  include  in  the  library  is  a  matter  of(trade-ofObetween  achiev¬ 
able  performance— and  planning  completeness— and  computational 
complexity;  a  typical  library  can  contain  hundreds  of  primitives.  The 
planner  in  [Frazzoli  et  al.  ’02]  used(625  primitives') while  maintaining 
real-time  computation  capabilities. 
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Invariant  tracking 


•  Let  us  consider  the  ^-invariant  system  x  =  f{x,  u)\  if  the  system  is 
unstable,  open-loop  control  is  doomed  to  failure. 

•  Close  the  loop  with  a  (static)  feedback  controller,  with  reference  v  g 
V: 

li:XxV^U. 

•  The  MA  approach  is  applicable  as  long  as: 

-  theCfeedback  preserves  invariance) 
i.e.  if  x  =  f{x,  ii{x,  v))  =  f{x,  v)  is  ^-invariant,  and 

-(Closed  paths  on  the  MA  lead  to  contraction  mappings.) 

•  Note  that  an  open  maneuver  sequence  is  allowed 
to  be  ’’destabilizing.” 

•  For  the  helicopter  example,  we  used  a  ’’backstepping  on  manifolds” 
approach  introduced  in  Frazzoli  et  al,  2000,  that  satisfies  the  above 
assumptions  (for  appropriate  choice  of  maneuvers). 
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Maneuvers 


•  Simple  transitions  between  different  trim  primitives  can  be  generated  by  commanding 
a  transition,  over  a  time  T,  in  the  velocity  of  the  reference  trajectory. 

•  The  closed-loop  behavior  of  the  helicopter  will  provide  a  feasible  trajectory  achieving 
the  desired  velocity  change. 

•  The  choice  of  the  time  T  determines  the  “aggressiveness”  of  the  maneuver,  and  is 
tuned  to  achieve  a  fast  response,  without  violating  flight  envelope  constraints. 


Maneuver  Examples 


North  [m] 


East  [m] 


Transition  from  hover  to  forward  flight 


Transition  from  forward  flight  to  steady  turn  to  the  left 
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Aerobatic  Maneuver 


Time  [s/10] 

Ag-turn  (or  Hammerhead) 


North  [m] 
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Maneuver  Automaton 


Steady  left  turn 


Steady  right  turn 


ID 

Pred 

Succ 

Duration  [s] 

Ap 

Aipl^ 

a 

a 

/? 

7.5 

(67.5,  0,  0) 

0 

b 

p 

a 

5 

(22.5,  0  0) 

0 

c 

(3 

7 

4.5 

(31.5,  -41.7.0,  0) 

-120.0 

d 

7 

/? 

2 

(28.9,  -6.6,  0) 

-15.0 

e 

/? 

(5 

4 

(34.2,  34.9,  0) 

105.0 

f 

(5 

P 

2.5 

(36.1, 8.6,  0) 

15.0 

g 

/? 

(3 

7.1 

(-43.5,  0,  0) 

180 
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Search  for  Optimality 
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Match  Racing 


The  sailor’s  problem 


•  Problem:  Steer  a  sailboat  between  two  waypoints  in  minimum  time. 

•  Dynamic  model:  A  sailboat  can  be  modelled  as  a  hull  and  three 
wings:  the  sail,  the  keel,  and  the  rudder.  Propulsive  forces  are 
generated  by  exploiting  the  relative  motion  of  air  and  water. 

•  Controls: 

-  Tiller  (rudder).  Positive  if  turning  into  the  wind.  Stall  condition 

\Ui  -  ^waterl  <  17°. 

-Sheet  (mainsail  angle).  U2  G  [10°,85°],  note  that  |^saii|  = 
min(6>wind,  ^2),  i-©-  the  sheet  can  only  pull  the  sail,  cannot  push 
it  against  the  wind. 

•  Two  unconnected  regions  of  operation,  i.e.  starboard  and  port 
tacks  (wind  on  either  side  of  the  boat). 

•  The  system  is  invariant  under  translation  and  reflection  about  the 
wind  axis  ^  the  symmetry  group  is  not  connected. 
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MA  design 


Aerospace  Robotics  and  Embedded  Systems  Laboratory 


Simulation  results 


QuickTime™  and  a 
decompressor 

are  needed  to  see  this  picture. 


•  Negligible  online  computation  time 

•  Optimal  time  (using  the  MA  language):  177  seconds. 

•  Explicit  solution  effectiely  provides  a  feedback  control  policy,  e.g., 
providing  robustness  w.r.t.  environmental  disturbances. 
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An  enabling  tool  for  real-time  motion  planning 


•  “Symbolic”  trajectory  generation  compatible  with 
state-of-the-art  algorithms  for  motion  planning. 

-  Incremental  sampling-based  search  algorithms  (RRTs, 
LaValle  &  Kuffner])  implemented  successfully  (e.g.,  on  the 
JCI_A/Golem  group  DARPA  Grand  Challenge  vehicle). 

•  Working  with  a  carefully  chosen  library  of  “natural  trajectory” 
isolates  motion  planning  from  safety/stability  concerns. 

•  Real-time  safety  guarantees  in  uncertain  environments. 
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Numerical  Experiment  Results 
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Motivation 

■  The  faster  the  better 

■  But  how  fast? 
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The  Challenges 


Terrain  profile:  must  have  a  good  3D  map 

Obstacies  avoidance:  does  not  apply  to 
rough  terrain 

Vehicle  stability:  depends  on  slope, 
curvature,  and  speed 

Soil  properties:  may  limit  ability  to  traverse 
a  given  terrain  segment 

Computational  efficiency:  avoid  searching 
in  the  state-space 

Moving  obstacles:  avoid  other  vehicles 
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This  talk 


■  Describe  a  unified  physics-based 
planner  that  addresses 

o  Vehicle  stability 
o  Soil  properties 
o  Obstacle  traversal 
o  Online  navigation*  (lagnemma) 
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Vehicle  Stability 


4k 

|f 
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Stability 


Dynamic  instability 


Static  stability 
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The  Problem 


■  Where  is  the  vehicle  statically 
unstable? 

■  At  what  speed  is  the  vehicle 
dynamically  unstable? 
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Approach 


■  Define 

o  Static  stability:  acceleration  range  at  zero  speed 

o  Dynamic  stability:  max  speed  that  does  not 
violate  dynamics  constrains 

■  Map  constraints  on  ground  forces  to 
constraints  on  speed  and  acceleration 

■  Determine  static  and  dynamic  stability 
margins  from  attainable  speeds  and 
accelerations 
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T reated  so  far 


■  Suspended  point  mass 

■  Planar  rigid  body 
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Planar  Vehicle  Model 


■  A  planar  all  wheel  drive 

■  3  DOF  (x,y,0) 

■  2  ground  forces  (4  components) 

■  Equations  of  motion: 


Fi  +  F2=m(x-g) 

X  Fj  +  r2  X  F2  =  Ico 


F  ^  F  +  ^2 
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Moment  Equation 

Moment  equation  becomes  an 
equality  constraint 


rj  X  Fj  +  r2  X  F2  +  r3  X  mg  =  0 


■  External  forces  must  produce  a 
zero  moment  around  ZMP 


■  ZMP  reflects  body  inertia  and 
path  curvature 


5  October  2006 


ARO  Wrkshop  on  Mobility  and  Control  in  Challenging  Environments 


11 


Dynamic  Constraints 


■  7  constraints: 

o  6  force  inequality  constraints 
o  1  moment  equality  constraint: 

/i  </^i 

/i  > -fjRx 
/2  </^2 
>  -/^2 
7?i>0 
>0 


rj  X  Fj  +  r2  X  ^2  +  Tj  X  mg  =  0 
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Constraints  on  Speed  and 
Acceleration  I 


■  Pick  any  2  force  equality  constraints 

F  =f  e 


=  f  G 


■  Express  forces  in  terms  of  eg 
acceleration 

■  Substitute  in  moment  equation 

■  Obtain  a  line  in  x-y  plane 

ax  +  by  +  c  =  Q 
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Constraints  on  Speed  and 
Acceleration  II 


■  Inequality  part  of  constraints  maps  to  a  half  plane  in 
x-y  plane 

■  Intersecting  all  half  planes  produces  the  set  of 
admissible  accelerations 
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Map  eg  acceleration  to  path 
coordinates 
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Dynamic  Stability  Margin 


■  Maximum  speed:  reflects 
curvature,  slope  and  friction 
constraints 


S 
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Static  Stability  Margins 


■  Maximum  symmetric 
acceieration  range  at  zero 
speed:  refiects  siope  and 
friction  constraints 


S 


Acceleration  limit 
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Statically  Unstable 


Vehicle  cannot  sustain  its  position  at  zero 
speed 
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Example 


sdot' 


Concave 


4 

2 


-2  - .  : . - 

-4  - .  :  - 

-6  - . . ; .  . - 

-8  - . i . ^ . - 

-10  - . . ^ .  . - 

-12  I  I  i  i  i 

0  10  20  ,  2  30  40  50 

sdot"" 


Flat  incline 


Convex 
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Example:  Sinusoidal  Path,  all 
wheel  drive 


Rigid  body 

Suspended  point  mass 
Point  mass  1 


J _ ^ _ I _ ^ _ I _ L 


0  0.5  1  1.5  2  2.5  3  3.5  4  4.5  5 


Static  Stability  Margins 


Dynamic  Stability  Margins 


X  of  c.g.  (m) 
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Soil  Properties 
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The  Problem 


■  At  what  speed  can,  or  should,  the 
vehicle  move  on  sandy  soil? 

■  What  is  the  steepest  slope  it  can 
climb? 
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Approach 


■  Incorporate  wheel/ground  model  into 
the  stability  analysis 

■  Brixius  model: 

Brixius,  W.  W,  1987.  Traction  prediction  equations  for  bias  piy 
tires.  ASAE  paper  no.  87-1622,  ASAE,  St.  Joseph,  Ml  49085. 

■  Focus  on  a  longitudinal  model 
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Ground  forces 


■  Net  traction:  nt  =  gt-mr 

■  Net  braking:  nb  =  gt+mr 
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Brixius  Model 


Cone  index  Cl 
Mobility  number  Bn 


= 


CI‘b‘d 


1  +  5- 
_ ^ 

1  +  3^ 

V  d  J 


Slip  ratio  s 

^  =  1 - ^ 

r.co 

Net  traction 

\j 

NT  =  0.88tV(l  -  )(1  -  e"’  )  - 

1 

[Bn 

Net  braking 

/ 

NB  =  -0.881V(1  -  )(1  - 

1 

\ 


+ 


Bn  4K 


V 


J 
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Net  Traction/Braking 
Coefficient 


Net  Traction  Coefficient 


Net  Braking  Coefficient 
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Example:  effect  of  soil 
Droperties 
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Obstacle  Traversal 
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The  Problem 

■  Avoid  or  climb? 

■  Not  just  a  kinematic  problem 
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Approach 


■  Model  terrain  and  obstacles  by  a 
smooth  representation 

■  Introduce  a  continuous  traversability 
measure  based  on  dynamic  stability 

■  Maximize  traversability 

■  Minimize  motion  time 
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Terrain  Representation 


■  Represent  surface  by  a  smooth  B- 
patch 

■  Embed  obstacles  in  the  B-patch 
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Traversibility  Measure 


■  Traversability  =  dynamic  stability  margin 

■  Cost  for  a  path  segment 
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Motion  Planning 


Represent  terrain  by  a  2D  grid 


ds 


Compute  —  for  each  edge 


m 


Search  for  a  set  of  shortest  traversable  paths 


.  r  ds 
inj  — 

J  e 


mm 


m 


Traversable  path  is  the  initial  guess  to  a  local 
time  optimization 


min 
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Example 


Shortest  Path 
(Laubach,  JPL) 


Best  traversable  Path 
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Demo 
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Hazard  Avoidance  for  High-Speed 
Unmanned  Ground  Vehicies  in 

Rough-Terrain 
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Assumed  Scenario 


Pre-planned  path 
Onboard  sensors 

-  Range  sensor 

-  Inertial  navigation  sensor 

-  GPS 

Vehicle  speed  (and  terrain)  can  cause 
slip,  ballistic  motion,  and  roll  over 

A  priori  knowledge 

-  Vehicle  parameters 

•  Inertia,  stiffness,  mass 

-  Topographical  map 

-  Large-scale  soil  type  estimate 

-  Terrain  roughness  estimate 
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Research  Challenges 


•  Dynamically  feasible 

•  Computationally  efficient 

•  Vehicle/terrain  interaction  effects 

•  Uncertainty  in  the  terrain  profile 

•  Applicable  in  highly  unstructured  environments 

•  Hazards  are  not  solely  binary  manner 

•  Consider  vehicle  characteristics 


Proposed  Solution 


Terrain  Roughness 
Terrain  Roll 
Terrain  Pitch 
Terrain/Tire  Coefficient  of  Friction 

Obstacle  Location 
Obstacle  Type 

Path  Curvature 
Velocity 

Vehicle  Parameters 


Online 

Hazard 

Avoidance 

Algorithm 


Modified 
Path  and 
Velocity 


The  Trajectory  Space 


•  The  trajectory  space  is  a  compact  representation  of  a 
vehicle’s  performance  limits 
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Dynamic  Constraints 


•  Power  train  constraints 

-  Engine 

-  Terrain  pitch 

-  Aerodynamic  drag 

-  Rolling  resistance 


0  2  4  6  8  10 


Velocity  (m/s) 


•  steering  constraints 

-  Tire  cornering  stiffness 

-  Center  of  mass  location 

-  Wheelbase 

-  Steering  angle 


Curvature  (m' 


Dynamic  Constraints 


K 


max,  mm  _ 

steering 


Velocity  (m/s) 


Rollover  constraints 

-  Vehicle  properties 

-  Track  width 

-  Sprung/  Unsprung 
mass  height 

-  Suspension  properties 


Dynamic  Constraints 


K 


max,  min 
rollover 


{d-hy- hj)g^  ±{h  +  dy)g^ 
{h  +  dy)v^ 


Velocity  (m/s) 


•  Sideslip  constraints 

-  Terrain  inclination 

-  Traction  coefficient 


Curvature  (m‘ 


Dynamic  Constraints 


mm,  max 
^slip 


-  gx  ±  Mg. 


Velocity  (m/s) 


Dynamic  Trajectory  Space,  r 


•  The  set  of  velocity  and  curvature  pairs  that  are 
dynamically  admissible  on  a  given  terrain  patch 


Q  Steering  Sideslip  Rollover 

Constraint  Constraint  Constraint 


Velocity  (m/s) 


Maneuvering  Inside 
Trajectory  Space  Constraints 
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Maneuvering  Outside 
Trajectory  Space  Constraints 
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Effect  of  Terrain  Conditions 


Roll  =  0° 

,  |li=0.6  to  1 .5 

_ : _ 1 _ 

Roll  Limit 

Slip  Limit 

Velocity  (m/s) 
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Effect  of  Terrain  Unevenness 


Trajectory  Space;  Pitch  =  0°;  Roll  =  20  to  40°,  |li=1.2 


Cui  valure  (m 


Dynamic  Trajectory  Space 


Reachable  Trajectory  Space,  A 


The  set  of  admissible  velocity  and  curvature  pairs  a 
vehicle  can  transition  to  in  a  given  time,  t. 
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The  Admissible  Trajectory 

Space,  0 


The  intersection  of  the  dynamic  trajectory  space  and  the 
reachable  trajectory  space: 


Hazard  Trajectory  Space,  Q 


•  The  hazard  trajectory  space  consists  of  velocity 
and  curvature  pairs  that,  if  maintained,  result  in 
intersection  with  the  hazard 


Percentage  of  Safe  T reversals  over  a  Ditch 


Roughness 
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Roughness  and  the  Trajectory 

Space 


Trajectory  Space  with  Ditch  -  Flat 


Hazard  Avoidance  Maneuver 
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When  to  Enact  a  Hazard 
Avoidance  Maneuver 


Maneuver  Selection 


•  Let  the  total  admissible 
trajectory  space  be  defined 
as: 

Z  =  (0in...n0j-Qi 


Find: 


•  Many  possible  methods 


-  Discretize  space 

-  Minimize  A 


Path  Resumption  Maneuver 
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Hazard  Avoidance  on  Rough 
Terrain  Simulation  Results 


Hazard  Avoidance  on  Rough 
Terrain  Simulation  Results 


6 

5 

4 

3 

2 
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-2 

-3 
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Hazard  Avoidance  on  Rough 
Terrain  Simulation  Results 
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Autonomous  Rough  Terrain 
Experimental  System  (ARTEmiS) 


Inertial  Navigation 
System  (Hidden) 

PC104  Onboard 
Computer  ^ 

Tachometer 

Outriggers 


Experiment  Title 

Purpose 

Multiple  Hazards 

Demonstrate  high  speed  avoidance  of  serial  hazards 

Sloped  Terrain 

Sloped  terrain  affects  choice  of  maneuver 

Rough  Terrain 

Demonstrate  algorithm  on  rough  terrain 

DGPS 


Engine 


Emergency 
Kill  Switch 
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Multiple  Hazard  Avoidance 
Experimental  Results 


X(m) 


Trajectory  Space  for  First  Obstacle 


Velocity  (m) 
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Multiple  Hazard  Avoidance 
Experimental  Results 


15 

10 

5 

P  0 

Obstacles 

—  u 

>  -5 

-10 

-15 

-  Desired  Path 

-  Actual  Path 

X  Second  Obstacle  Detected 

^ - 1 - i - ^ 

1 

I 

C 

)  20  40 

60 

X(m) 

80 

100 
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Rough  Terrain  Experimental 

Results 


•  Rough  natural  terrain 

•  Experiments  run  at  speeds  of  4  to  7  m/s 

•  Ballistic  motion  and  wheel  slip  achieved 


Vertical  Acceleration  for  Flat  and  Rough  Terrain 


>•  T* 


>  •  .  -f* 


-  Rough  Terrain 

-  Flat  Terrain 


8  10  12  14 

Time  (s) 


Rough  Terrain  Experimental 

Results 


Rough  Terrain  Experimental 

Results 
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Conclusions  and  Future  Work 


•  An  effective  physics- 
based  hazard  avoidance 
algorithm  for  emergency 
situations 

•  Extensions  for 
omnidirectional  vehicles 

•  Improved  maneuver 
selection 
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Sampling  Based  Model  Predictive  Control 


KEY  FEATURES: 

>Model  Predictive  Control  (MPC)  accomplishes  motion  planning  in  the 
presence  of  constraints  using  dynamic  models. 

>These  constraints  may  include: 

-  obstacles,  stability  constraints,  limitations  on  actuator  amplitude, 
maximum  vibration  amplitude,  etc. 

>The  dynamic  models  allow  path  planning  to  rigorously  take  into  account: 

-  vehicle  kinematics,  slip,  energy  consumption,  complex  motion  (e.g., 
when  climbing  complex  objects),  terrain  type,  the  calculation  of  time- 
dependent  paths  (position,  velocity,  &  acceleration),  dynamic  obstacles, 
etc. 

>A  key  step  in  one  approach  to  making  MPC  computationally  tractable  for 
AGVs  is  the  sampling  of  the  model  input  space  (usually  consisting  of  forces 
and  torques). 

>  Sampling  Based  MPC  can  also  exploit  “differential  flatness”  for 
computational  efficiency. 


Receding  Horizon  MPC  vs  Shrinking  Horizon  MPC 


N  M-l 

mini = yi|r(A: + i)  -y(k + + /)||^ 

“  i=l  /=0 

Horizon  Window 
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Optimization  Problem  for 
Sampling  Based  MPC  (SBMPC) 


Given  a  system  model: 

x{t)  =  x(0)  =  Xq 

y{t)  =  h{x{t)) 

solve  the  optimization  problem : 

min  J  =  YQiyM  -  )  +  Z 

i=\  i=\ 

^ - V - ^ 

Distance  to  Goal  (for  Q=I) 

subject  to 

e  G  for  A:  =  iV  or  A:  = 

(avoid  obstacles,  satisfy  velocity  constraints,  etc.) 
u,  El\u  u  1  V A: 

k  max  J 


SBMPC  Predictive  Control  Overview 


^k+\ 


Focus  on  the  Optimization  &  Prediction  Process 


•The  following  slides  focus  on  the  optimization  and  prediction 
process  using  input  sampling: 


Optimization 

min  J 
u 

& 

Prediction 


•The  optimization  is  A*  optimization. 

-It  will  always  yield  the  global  minimum  subject  to  the  constraints  of 
input  sampling. 

-The  algorithm  is  resolution  complete. 

-As  the  sampling  increases,  (if  done  properly)  a  feasible  path  will  be 
found  when  one  exists  with  probability  one. 

-Computational  speed  can  be  greatly  increased  for  some 
applications  (e.g.,  high  speed  maneuvering  on  a  flat  surface)  by 
precomputing  the  A*  costs). 

•Because  the  optimization  is  performed  repeatedly,  it  can  benefit 
computationaiiy  from  a  variation  of  A*  called  “Dynamic  A*.” 


Sampling  Based  Model  Predictive  Optimization 


Sampling  Based  Model  Predictive  Optimization 
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Sampling  Based  Model  Predictive  Optimization 
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Preliminary  Simulation  Results: 
Paths  to  Goal 


Goal  Region 


X  Position 


Y  Position 


Preliminary  Simulation  Results: 
Obstacle  Free  and  Time  Optimal  Paths 
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Preliminary  Simulation  Results 
Distance  Optimal  Paths 
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Problems  to  Be  Solved  Using  Dynamic  Modeis 


>Path  planning  for  climbing  steep 
hills. 

-Need  to  plan  velocity  needed  to  reach 
the  top. 

-A  similar  problem  is  rocking  a  vehicle 
out  of  a  ditch. 


Problems  to  Be  Solved  Using  Dynamic  Modeis 


>Path  planning  for  climbing  steep 
hills. 

-Need  to  plan  velocity  needed  to  reach 
the  top. 

-A  similar  problem  is  rocking  a  vehicle 
out  of  a  ditch. 

>Path  planning  for  high  speeds. 

-Important  when  travelling  at  high 
speeds  around  obstacles.  (Slip  needs  to 
be  taken  into  account.) 

-May  allow  a  vehicle  to  emulate  the 
efficient  curve  traversal  of  race  car 
drivers. 


Problems  to  Be  Solved  Using  Dynamic  Modeis  (Cont’d) 


>  Planning  for  Stability 

-Stability  at  high  speeds  and  for  small 
turn  radii  is  essential  for  vehicle  safety. 


>  Energy  Efficient  Path  Planning 

-This  research  will  involve  the  use  of 
dynamic  models  to  develop  more 
accurate  measures  of  the  energy  used  to 
navigate  a  given  path. 

-This  is  especially  important  in 
undulating  environments  with  different 
terrain  types. 


Problems  to  Be  Solved  Using  Dynamic  Modeis  (Cont’d) 


>  Obstacle  Traversal 

-This  research  will  feed  directly  into  our 
research  on  Control  on  Difficult 
Terrains. 

>Path  Planning  in  the  Presence  of 
Mechanical  Failure 

-An  example  of  this  is  a  flat  tire. 

-In  this  case  the  model  changes  and  so 
may  the  most  efficient  paths. 

>Path  Planning  in  the  Presence  of 
Dynamic  Obstacles 
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Part  I: 

Constrained  Trajectory  Planning  on 

Outdoor  Terrain 
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Motion  Planning 


□  Given: 

■  wheeled  robot 

■  coarse  uneven  terrain  map 

■  dynamic  constraints 

□  Compute: 

■  shortest  time  feasible  trajectory  to  a 
goal  configuration 


Problem 


Segway  RMP 


Assumptions 


□  Dynamics 

-  detailed  physics-based  simulation  too  costly 

-  empirically  determine  safe  bounds 

-  assume  robot  wheels  do  not  slip  or  slide 

-  assume  the  existence  of  a  controller  that  can 
achieve  controls  in  the  vehicle  envelope 

-  Example:  Segway  RMP 

■  PID  for  velocity  control, LQR  for  balancing 


□  Planar  Discretization 

Over  a  short  path  segment  6g  on 
terrain  surface  G  cz Redefine: 

-flat  patch  n«{p'  Ip'  = 

-  local  ref.  frame  e  (Gx  SO(3)) 

-  the  robot  transitions  between  patches 
after  traveling  distance  of  length  6g 


Discretized  path 


Robot  Model  and  Constraints 


□  Simple  differential  drive  model 


/  X  \ 

V  cos  ■0  \ 

/  0  0  \ 

ii 

V  sin  ■0 

0  0 

tj 

+ 

0  0 

■i' 

0 

1  0 

\  ii'  / 

0  / 

\  0  1  ) 

■  x,  y,  Ip  are  position  and  orientation  with  respect  to  frame 

■  v.o.'  are  forward  and  angular  velocities 

■  in  addition  we  keep  track  of  pitch  0  and  roll  c6  computed  from  the 
planar  patch  incline 


□  Bounds: 


■  curvature: 


■  dynamic: 


u.'  <  Vn-^ax(:v) 

Velocity:^;  <  ^v^ax■  stability: 


Acceleration:  u  <  a-r^rax 


I?.ol  1 : 


Sampling  Approach 


□  Control-system  based  Probabilistic  RoadMaps  (Hsu  et.  al.) 

■  near  optimal  solution 

■  near  real-time  performance 

□  Handle  the  dimensionality  (NP-hard) 

□  Efficiently  explore  the  state  space  by  building  a  tree  of 
nodes  (connected  with  feasible  trajectories)  until  the  goal  is 
reached 

□  Nonholonomic  constraints  automatically  satisfied  by  the 
forward  model 


Randomized  Kinodvnamic  Solution 


□  Control-system  based  Probabilistic  RoadMap 

■  sampling  in  position  space 

■  probabilistic  and  resolution  complete 


□  Implementation:  based  on  Frazzoli,  Dahleh,  Feron,  2000 

■  expansion  heuristics  (A*-like) 

■  pruning  techniques 


PRM  on  artificial  terrain 


Local  Steering  Method 


□  How  is  the  system  steered  towards  new  milestones? 

■  terrain  induces  a  velocity  and  acceleration  limits 

■  curvature  constraints  further  limit  the  control  choice 

■  Choose  bang-bang  controls  within  the  dynamic  bounds  that  satisfy 
the  curvature  constraints 

■  Hard  to  determine  time-optimal  switching  times 

□  Decoupled  approach: 

■  find  the  shortest  path  that  satisfies  the  curvature  constraint:  clothoid 
with  trapezoidal  curvature  profile 

■  bang-bang  angular  acceleration  control  along  curved  path  segment 

■  bang-bang  linear  acceleration  control  along  straight  path  segment 

■  not  guaranteed  to  be  optimal  but  a  good  choice  locally 

□  A  more  optimal  solution  can  be  found  numerically 
(see  second  part  of  the  talk) 


Simulations 


□Trajectory  Cost  (left  y-axis)  vs.  Map  Size 
□  Computation  Time  (right  y-axis)  vs.  Map  Size 


□  Good  convergence  and  runtime  in  large  maps 


seconds 


Simulations 


□Trajectory  Cost  (left  y-axis)  vs.  %  trapped  nodes 
□  Computation  Time  (right  y-axis)  vs.  %  trapped  nodes 


□  Efficiently  finds  near-optimal  trajectory  in  terrains  of 
different  expansiveness 


Robot  Experiments 


Blue  -  computed  path 
Red  -  executed 


Blue  -  angular  velocity  profile 
Red  -  linear  velocity  profile 


rad/s 


Conclusions 


□  Relaxing  some  of  the  requirements  for  terrain 
modeling  allows  for  efficient  kinodynamic  planning 

□  The  solutions  are  not  necessarily  feasible  but  are 
practical  when  precise  terrain  maps  are  unavailable 

□  More  accurate  models  are  required  to  produce  fully 
executable  paths 

□  In  the  future,  we  will  focus  on  employing  better 
models  without  losing  near  real-time  performance 


Part  II: 


Optimal  Motion  Controi  of  Nonhoionomic  Systems 


Motion  Planning  and  Constrained  Optimization 


□  Consider  systems  with  drift  and  nonintegrable  velocity 
constraints,  e.g.  a  car-like  robot  moving  at  high  speed 

□  One  way  to  compute  locally  optimal  motions  is  to  solve  a 
nonlinear  constrained  optimization  problem 

□  Discretize  the  equations  of  motion  and  use  them  as 
constraints  in  an  optimization  of  a  given  cost  functional 

□  Any  additional  constraints  are  expressed  as  (in)equality 
constraints  on  the  configurations  and  velocities 

□  The  solution  is  a  discrete  trajectory  and  a  discrete 
control  curve  (or  a  finite  set  of  control  parameters) 


Motion  Planning  and  Constrained  Optimization 


□  Some  recent  examples  in  robotics: 

□  Milam,  Mushambi,  Murray  -  constrained  trajectory  generation, 
differential  flatness 

□  Kelly,  Nagy,  Howard  -  parametric  optimal  control,  rough  terrain 

□  Cheng,  Frazzoli,  LaValle  -  improving  precision,  closing  gaps, 
exploiting  symmetries 

□  Dever,  Mettler,  Feron,  Popowc- trajectory  generation, 
parameterized  maneuver  classes 

□  Lamiraux,  Bonnafous,  Lefebvre  -  path  deformation 

□  All  optimization  approaches  have  one  common  aspect 
there  is  some  form  of  discretization  of  the  dynamics  (e.g. 
when  integrating  the  equations  of  motion  or  when 
enforcing  the  constraints) 


Roboties 
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Discrete  Mechanics 


□  A  recently  developed  theory  for  discretizing  the 
dynamics  of  physical  systems 

□  Based  on  the  discretization  of  variational  principles^ 
(roots  in  the  discrete  optimal  control  from  the  1960’s) 

□  Results  in  higher  order  integrators 

□  Preserves  Structure:  symplectic  and  momentum 
conservation  (in  the  absence  of  forces),  approximately 
respects  the  energy  balance 

□  Discrete  reduction  analogs 

□  Performs  well  in  both  conservative  and  forced  systems 


^  J.  Marsden  and  M.  West,  “Discrete  mechanics  and  variational  integrators”,  Acta  Numerica,  2001. 


Discrete  Mechanics 


Space  Mission  Design  (Junge  et  al,  2005) 


Elasticity  Simulation  (Kharevych  et  al,  2006) 


Optimal  flapping  strokes  (Ross,  2005) 


Optimal  motions  in  fluid  (Kanso  et  al,  2005) 


Nonsmooth  finite  element  contacts 
(Cirak,  West,  2005) 


Motion  planning  with  Nonholonomic 

Discrete  Mechanics 


N 

Convergence  vs.  Number  of  Segments  (N) 


□  Example:  car-like  robot  among  obstacles^ 

□  Improved  efficiency  and  convergence  over 
standard  collocation  method  (graphs) 


4  fc  ifc  ir?  M  !B  ^  !:iJ 

Execution  Error  vs.  Ndmber  of  Segments  (N)  q  aIIows  coarser  discrotization  (larger  time  steps) 


4  Marin  Kobilarov,  Gaurav  Sukhatme,  “Optimal  motion  control  of  nonholonomic  systems”,  2006,  preprint 


Discrete  Variational  Principle 


□  A  path  in  a  configuration  manifold  Q  is  represented  by  a 
discrete  sequence  of  points 

□  The  tangent  bundle  TQ  replaced  by  the  product  Q  x  Q 


□  The  nonholonomic  distribution  V  c  TQ  replaced  by  a 
discrete  analog  Vd  g  Q  x  Q 


Discrete  Variational  Principle 


□  Approximate  the  action  integral  between  two 
consecutive  points  using  discrete  Lagrangian: 


ik+l)h 


Ldilk,  Qk+l  )  " 


kh 


□  The  virtual  work  of  control  force  /  :  [o,  r]  ^  t*q  is 
approximated  on  each  segment  by: 

Ak+l)h 

fk  ■  +  fk  ■  ^<lk+i  ~  /  fit]  ■  Sq(t}dt 

Jkh 


where  fkJk  e  t*q  are  called  /effand  r/gW  discrete 
control  forces 


Discrete  Lagrange-D’Alembert  Principle 


□  The  discrete  nonholonomic  Lagrange-D’Alembert  principle^ 
can  be  derived  as: 

A"-l  A"-l 

<5  ^  Ld{qk,qk+i)  + '^  fk  ■  ‘5'Jfc  +  fk  '  ^ 

fc=0  fc=0 

Sqo  =  =  0  Sqk  e  'D^^,  ((?fc,gfc+l)  e  'l^d  for  all  k  =  0, 


□  Expressing  the  constraints  as  ^-5“  -  =  0_  cj  -  ir,s) 

the  discrete  nonholonomic  Euler-Lagrange  equations  read: 


^  +  ifk  )«  +  ifk-1  )<»  “ 


(dlk  . 


ih% 


” '  I  /  ■f’?-  1  I  f  tS-\-  \ 


-0,  where  :  Q  x  Q  ^  M  define  'Dd 


2  J.  C.  Monforte,  Geometric,  Control  and  Numerical  Aspects  of  Nonholonomic  Systems.  Springer,  2002. 


Discrete  Nonholonomic  Optimal  Control 


For  optimal  control  we  discretize  the  cost  functional^: 

[  Ciq{t),q{t),fit)}dt 
Jo 


using  a  quadrature  approximation  on  each  segment 

Ak+i)h 

Cd{qk,qk+i,fkJk+i)-  C{q,q,f}dt 

Jkh 

to  derive  the  total  cost 


A 


+1 


O.  Junge,  J.  Marsden,  S.  Ober-Bldbaum,  “Discrete  Mechanics  and  Optimal  Control”,  IFAC,  2005 


Discrete  Nonholonomic  Optimal  Control 


□  An  example  discretization  scheme:  MIDPOINT  RULE 


+  /fc?  A+l) 

Qk  +  ^A+l  gA+1  —  Qk  fk  +  A+l 
2  ’  h  ’  2 

-t£i(®,«/fe+l)  =  «'i( - - , - T - ), 

,a( ^  \  1  Q'fc+l  \ 

- ! - 1 - ) 


), 


ik+i)h 


fit)  ■  6q{t)dt  fti  h 


2  ’  h 

fk  +  /fe+1 


kk 


2 


—  ^(A-  +  A+i)  •  ^Qk  +  ■jlA  +  A+i)  •  <5(/fc+i 

□  Other  discretization  schemes  are  possible  leading  to  higher  order 
integrators:  i.e.  sympiectic-partitioned  Runga-Kutta,  Veriet,  etc 


Discrete  Nonholonomic  Optimal  Control 


□  Optimization  Algorithm  Summary 

■  Represent  an  initial  trajectory  by  a  discrete  path  of  N  segments 

■  Form  the  discrete  cost  functional,  discrete  iagrangian,  discrete 
constraint  distribution,  and  discrete  analog  of  virtual  work 

■  Express  the  discrete  Euler-Lagrange  equations  as  constraints 

■  Add  boundary  conditions  and  any  other  constraints/bounds 

■  Solve  directly  using  Sequential  Quadratic  Programming 

□  Remarks 

■  The  equations  of  motion  are  replaced  by  their  discrete  variational 
counterpart 

■  The  algorithm  uses  only  algebraic  constraints  (no  derivatives) 

■  Good  performance  even  at  coarse  resolution  (big  time  steps) 

■  Potential  advantages  over  standard  “brute-force”  discretization 
used  in  standard  collocation,  shooting,  multiple  shooting  methods 


Discrete  Nonholonomic  Optimal  Control 


□  Summary 

■  Optimal  control  method  based  on  the  discretization  of  Lagrange- 
d’Aiembert  principle  of  virtual  work 

■  Resulting  discrete  variational  equations  and  discrete  nonholonomic 
constraints  of  motion  are  used  as  algebraic  constraints  in  a 
nonlinear  program 

■  We'*  are  able  to  show  improved  efficiency  over  standard  methods 

■  Discrete  Lagrangian  reduction  can  also  by  applied  to  simplify  the 
equations  of  motion  (in  the  presence  of  symmetries)  and  further 
improve  efficiency 

□  Many  possible  applications:  e.g.  complex  environments,  multiple 
vehicles,  natural  multi-resolution  methods,  global  search  ideas,  etc... 


4  Marin  Kobilarov,  Gaurav  Sukhatme,  “Optimal  motion  control  of  nonholonomic  systems”,  2006,  preprint 


ARO  Workshop  on  Mobility  and  Control  in  Challenging  Environments 


Olin  College,  Needham  MA 
October  5  &  6,  2006 

ATTENDEE  LIST 


University  Researchers 


Karl 

lagnemma 

MIT 

kdi(a)mit.edu 

Emilio 

Frazzoli 

MIT 

frazzolifijseas. ucla.edu 

Daniela 

Rus 

MIT 

rus(a)csail. mit.edu 

Kenjiro 

Tadakuma 

MIT 

John 

How 

MIT 

ihow(a)mit.edu 

Yoshiaki 

Kuwata 

MIT 

kuwataOJmit.edu 

John 

Leonard 

MIT 

ileonardOfM  IT.EDU 

Martin 

Udengaard 

MIT 

mru(a)mit.edu 

Dave 

Barrett 

Olin 

David.Barrett@olin.edu 

Gill 

Pratt 

Olin 

aill.Dratt@olin.edu 

David 

Miller 

Olin/OU 

Alonzo 

Kelly 

CMU 

alonzo@cmu.edu 

Chris 

Urmson 

CMU 

curmson(a)ri. cmu.edu 

Dim! 

Apostolopolous 

CMU 

da1v+@cs.cmu.edu 

Panagiotis 

Tsiotras 

GaTach 

D.tsiotras@ae.qatech.edu 

Ron 

Arkin 

GaTech 

ronald.arkin@cc.aatech.edu 

Efstathios 

Yelenis 

GaTech 

Joseph 

Ayers 

NEU 

lobster@.neu.edu 

Mark 

Rentschler 

UNL 

markrentschler(a)amail.com 

Ken 

Waldron 

Stanford 

waldron@cdr.stanford.edu 

Matthew 

Spenko 

Stanford 

msDenko(a)stanford.edu 

William 

T  ravis 

Auburn 

Dave 

Beviy 

Auburn 

dmbevlv(a)ena. auburn.edu 

Johann 

Borenstein 

UM 

iohannb(a)umich.edu 

Laura 

Ray 

Dartmouth 

laura.rav(a)Dartmouth.EDU 

Emmanuel 

Collins 

FSU 

ecollins(a)enq.fsu.edu 

Zvi 

Shiller 

Judea  and  Samaria 

shiller@vosh.ac.il 

Dan 

Lee 

Penn 

ddlee@seas.UDenn.edu 

John 

Steele 

Colorado  School  of  M 

isteele@mines.edu 

Marin 

Kobilarov 

use 

mkobilar@usc.edu 

Richard 

Voyles 

UMN 

vovles@cs.umn.edu 

Damian 

Lyons 

Fordham 

dlvons(a)cis.fordham.edu 

Eyad 

Abed 

Maryland 

abed@isr.umd.edu 

Dean 

Hougen 

Oklahoma 

houaen@ou.edu 

DoD  and  Other  Personnel 


Randy 

Zachery 

ARO 

randv.zacherv(a)us. armv.mil 

Russell 

Harmon 

ARO 

Russell.  HarmonSJus. armv.mil 

Tim 

Krantz 

ARL 

timothy.l.krantz@nasa.gov 

Marshal 

Childers 

ARO 

marshal.childers@us.armv.mil 

Jim 

Overholt 

TACOM 

OverholJ@tacom.armv.mil 

Alex 

Baylot 

ERDC 

Alex.Baylot@us.army.mil 

Christopher 

Cummins 

ERDC 

Christopher.L.Cummins@erdc.usace.army.mil 

Dave 

Horner 

ERDC 

David.A.Horner@.erdc.usace.armv.mil 

Gary 

Phettleplace 

CRREL 

Gary.E.Phetteplace@erdc.usace.army.mil 

Mike 

Perschbacher 

Griffin  Technologies 

mDerschbacher@snaD.ora 

Eric 

Krotkov 

Griffin  Technologies 

ekrotkov@ieee.org 

Rob 

Playter 

Boston  Dynamics 

Dlavter@.BostonDvnamics.com 

Jamie 

Anderson 

Draper 

iamie(a)draDer.com 

Amis 

Mangolds 

Draper 

amanaolds@draDer.com 

Adam 

Rzepniewski 

Draper 

arzepniewski@draper.com 

Greg 

Andrews 

Draper 

aandrews@draDer.com 

Paul 

Dibitetto 

Draper 

Ddebitetto@draDer.com 

Brian 

Yamauchi 

iRobot 

vamauchi@.irobot.com 

Brian 

Wilcox 

JPL 

Brian.  H.Wilcox(a)iDl.nasa.aov 

Gregory 

McKinney 

Vehicle  Control  Train! 

area@teamoneil.com 

Ed 

VanReuth 

DARPA 

evanreuth(a)darDa.mil 

Rich 

Weisman 

FMI 

rwiesman@.foster-miller.com 

Bill 

Hutchinson 

Behavior  Systems 

whutchi(a)behaviorsvs.com 

Gary 

Witus 

Turing 

witusg@umich.edu 

Wendell 

Sykes 

Context  Sys 

svkes@contextsvstems  .net 

